diff --git a/xo-kalmanfilter/.github/workflows/main.yml b/xo-kalmanfilter/.github/workflows/main.yml new file mode 100644 index 00000000..3c4faa53 --- /dev/null +++ b/xo-kalmanfilter/.github/workflows/main.yml @@ -0,0 +1,488 @@ +name: build xo-optionutil + dependencies + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) + BUILD_TYPE: Release + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-latest + + steps: + - name: checkout source + uses: actions/checkout@v3 + + - name: Install dependencies + run: | + echo "::group::install catch2" + # install catch2. see + # [[https://stackoverflow.com/questions/57982945/how-to-apt-get-install-in-a-github-actions-workflow]] + sudo apt-get install -y catch2 + echo "::endgroup" + + echo "::group::install eigen3" + sudo apt-get install -y libeigen3-dev + echo "::endgroup" + + #echo "::group::install pybind11" + #sudo apt-get install -y pybind11-dev + #echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-cmake + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-cmake + path: repo/xo-cmake + + - name: build xo-cmake + run: | + XONAME=xo-cmake + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-indentlog + uses: actions/checkout@v3 + with: + repository: Rconybea/indentlog + path: repo/xo-indentlog + + - name: build xo-indentlog + run: | + XONAME=xo-indentlog + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-refcnt + uses: actions/checkout@v3 + with: + repository: Rconybea/refcnt + path: repo/xo-refcnt + + - name: build xo-refcnt + run: | + XONAME=xo-refcnt + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-subsys + uses: actions/checkout@v3 + with: + repository: Rconybea/subsys + path: repo/xo-subsys + + - name: build xo-subsys + run: | + XONAME=xo-subsys + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-reflect + uses: actions/checkout@v3 + with: + repository: Rconybea/reflect + path: repo/xo-reflect + + - name: build xo-reflect + run: | + XONAME=xo-reflect + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-callback + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-callback + path: repo/xo-callback + + - name: build xo-callback + run: | + XONAME=xo-callback + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-printjson + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-printjson + path: repo/xo-printjson + + - name: build xo-printjson + run: | + XONAME=xo-printjson + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-webutil + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-webutil + path: repo/xo-webutil + + - name: build xo-webutil + run: | + XONAME=xo-webutil + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-randomgen + uses: actions/checkout@v3 + with: + repository: Rconybea/randomgen + path: repo/xo-randomgen + + - name: build xo-randomgen + run: | + XONAME=xo-randomgen + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-ordinaltree + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-ordinaltree + path: repo/xo-ordinaltree + + - name: build xo-ordinaltree + run: | + XONAME=xo-ordinaltree + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-reactor + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-reactor + path: repo/xo-reactor + + - name: build xo-reactor + run: | + XONAME=xo-reactor + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-statistics + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-statistics + path: repo/xo-statistics + + - name: build xo-statistics + run: | + XONAME=xo-statistics + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: Configure self (kalmanfilter) + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: | + XONAME=xo-kalmanfilter + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_MODULE_PATH=${PREFIX}/share/cmake -DCMAKE_PREFIX_PATH=${PREFIX} -DCMAKE_INSTALL_PREFIX=${PREFIX} -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + (cd ${BUILDDIR} && ctest -C ${{env.BUILD_TYPE}}) diff --git a/xo-kalmanfilter/.gitignore b/xo-kalmanfilter/.gitignore new file mode 100644 index 00000000..13c0afb7 --- /dev/null +++ b/xo-kalmanfilter/.gitignore @@ -0,0 +1,6 @@ +# clangd working space (see emacs+lsp) +.cache +# typical cmake build directory (source-tree-nephew) +.build* +# symlink to builddir/compile_commands.json; should be set manually in dev sandbox +compile_commands.json diff --git a/xo-kalmanfilter/CMakeLists.txt b/xo-kalmanfilter/CMakeLists.txt new file mode 100644 index 00000000..a49c8cb2 --- /dev/null +++ b/xo-kalmanfilter/CMakeLists.txt @@ -0,0 +1,30 @@ +# xo-kalmanfilter/CMakeLists.txt + +cmake_minimum_required(VERSION 3.10) + +project(xo_kalmanfilter VERSION 1.0) + +include(GNUInstallDirs) +include(cmake/xo-bootstrap-macros.cmake) + +xo_cxx_toplevel_options3() + +# ---------------------------------------------------------------- +# c++ settings + +# one-time project-specific c++ flags. usually empty +set(PROJECT_CXX_FLAGS "") +#set(PROJECT_CXX_FLAGS "-fconcepts-diagnostics-depth=2") +add_definitions(${PROJECT_CXX_FLAGS}) + +# ---------------------------------------------------------------- + +add_subdirectory(src/kalmanfilter) +add_subdirectory(utest) + +# ---------------------------------------------------------------- +# provide find_package() support for reactor customers + +xo_export_cmake_config(${PROJECT_NAME} ${PROJECT_VERSION} ${PROJECT_NAME}Targets) + +# end CMakeLists.txt diff --git a/xo-kalmanfilter/EXAMPLES b/xo-kalmanfilter/EXAMPLES new file mode 100644 index 00000000..8f69ea73 --- /dev/null +++ b/xo-kalmanfilter/EXAMPLES @@ -0,0 +1,164 @@ +scaffold kalman filter implementation here + +notation: + +x_(k) : n x 1 : true system state that we want to estimate +F(k) : n x n : state transition matrix at time t(k) +w_(k) : n x 1 : system noise, with mean 0, covariance Q(k) +Q(k) : n x n : covariance of systen noise + +v_(k) : m x 1 : observation errors at time t(k) +R(k) : m x m : observation error covariance matrix at time t(k) + +x(k) : n x 1 : state vector estimate for time t(k) +P(k) : n x n : covariance matrix for x(k) +z(k) : m x 1 : observation vector at time t(k) +H(k) : m x n : observation matrix at time t(k) + +K(k) : : kalman gain - measures information gain from observation z(k) + relative to prior P(k | k-1) + +use shorthand: + xT, F(k)T for transpose(x), transpose(F(k)) + x(k+1 | k) for "estimate x(k+1) given information known at t(k)" + +A. Linear Kalman Filter +----------------------- + +1. system model: + x_(k+1) = F(k).x_(k) + w_(k), w_(k) ~ N(0,Q) + + i.e. expected behavior of system from t(k) -> t(k+1), + absent system noise, is given by linear transformation F(k) + + ofc model is not directly observable, + since we don't know x_(k) or w_(k), + instead we will be estimating it. + +2. measurement model: + z(k+1) = H(k+1).x_(k+1) + v_(k), v_(k) ~ N(0, R) + +3. prior: + x(0), P(0) + + must be supplied as initial input + +4. pre-estimate for t(k+1) system state: + (before incorporating z(k+1) and accounting for system noise) + x(k+1|k) := F(k).x(k) + + in other words propagate t(k) estimate to t(k+1), + using F(k) + +5. pre-estimate for t(k+1) estimate covariance + (before incorporating z(k+1) and accounting for system noise) + P(k+1|k) := F(k).P(k).F(k)T + Q(k) + +6. kalman gain matrix + K(k+1) := P(k+1|k).H(k)T.inverse(H(k).P(k+1|k).H(k)T + R(k)) + + note that the matrix-to-be-inverted in the gain expression + is symmetric and positive-definite (so can use cholesky decomposition) + +7. innovation: difference between actual observation vector and + observation predicted from state estimate x(k+1|k): + z(k+1) - H(k+1).x(k+1|k) + +8. corrected state estimate for t(k+1): + x(k+1) := x(k+1|k) + K(k+1)[z(k+1) - H(k+1).x(k+1|k)] + +9. correct state covariance for t(k+1): + P(k+1) := [I - K(k+1).H(k+1)].P(k+1|k) + + +B. Extended Kalman Filter +------------------------- +Must write in continuous form, to deal with non-linearities + +notation: + +x_(t) : n x 1 : true system state that we want to estimate, + continuously evolves with t +f(x_(t),t) : n x 1 : expected derivative (d/dt)(x_(t)) +h(x_(t),t) : m x 1 : observation function +x(k) : n x 1 : state vector estimate for x(t) at time t=t(k) +F(x(t),t) : n x n : jacobian of f(x_(t),t) evaluated at x_(t)=x(t) +H(x(t),t) : m x n : jacobian of h(x_(t),t) evaluated at x_(t)=x(t) + +1. system model: + (d/dt)x_(t) = f(x_(t),t) + w(t), + where + w(t) is a white noise with + + / t2 + | + | w(t).dt ~ N(0, Q(t1,t2)) + | + / t1 + + editorial: + note that we exclude more general model + (d/dt)x_(t) = f(x_(t),t)) + G(x_(t),t).w(t) + because would be unable to assume RHS mean-square integrable. + The form chosen effectively band-limits the frequencies at which + w(t) acts, since it's independent of x_(t). This allows mean-square + rules to be applied as in linear kalman filter + +2. measurement model: + z(k+1) = h(x_(t(k+1)), t(k+1)) + v(k), v(k) ~ N(0, Rk) + +3. prior: + x(0), P(0) + + (same as for linear kalman filter) + +(3a). state estimate propagation model: + (d/dt)x(t) = f(x(t),t) + + editorial: + this step is *sketchy*: x(t) is intended to be minimum-variance + estimate for system state, but that property is lost when solving + differential equation (d/dt)x(t) = f(x(t),t) with non-linear f. + +(3b). covariance estimate propagation model: + (d/dt)P(t) = F(x(t),t).P(t) + P(t).F(x(t),t)T + Q(t) + +4. pre-estimate (extrapolation) for t(k+1) system state: + + can use first order approx: + x(k+1|k) = x(k) + F(x(tk),tk).(t(k+1) - t(k)) + or multiple timesteps if t(k+1) - t(k) is too large + + i.e. approximately solving (3a) + +5. pre-estimate (extrapolation) for t(k+1) error covariance: + + P(k+1|k) = P(k) + [F(x(tk),tk).P(k) + P(k)F(x(tk),tk)T + Q(tk)].(t(k+1) - t(k)) + or multiple timesteps if t(k+1) - t(k) is too large + + i.e. approximately solving (3b) + +6. kalman gain matrix + K(k+1) = P(k+1|k) + .H(k+1)T(x(k+1|k)) + .inverse[H(k+1)(x(k+1|k)).P(k+1|k).H(k+1)T(x(k+1|k)) + R(k+1)] + +7. innovation: + z(k+1) - h(x(k+1|k), t(k+1)) + +8. corrected state estimate for t(k+1): + x(k+1) = x(k+1|k) + K(k+1).(z(k+1) - h(k+1)(x(k+1|k)) + +9. corrected error covariance for t(k+1): + P(k+1) = [I - K(k+1).H(k+1)(x(k+1|k))].P(k+1|k) + + editorial: + This step is *sketchy*, to the extend h() is non-linear, linearizing around + x(k+1|k) may give poor estimate of state covariance + +10. linearization (1st order term for taylor series of f() around x(t)) + jacobian: + F(x(t),t) = (df/dx_) evaluated at x_(t)=x(t) + + H(k+1)(x(k+1|k)) = (dh/dx_) evaluated at x_(t)=x(k+1|k) + diff --git a/xo-kalmanfilter/README.md b/xo-kalmanfilter/README.md new file mode 100644 index 00000000..fd4ef52d --- /dev/null +++ b/xo-kalmanfilter/README.md @@ -0,0 +1,56 @@ +# kalman filter library + +linear kalman filter implementation. + +## Getting Started + +### build + install dependencies + +build+install these first + +- xo-reactor [github.com/Rconybea/xo-reactor](https://github.com/Rconybea/xo-reactor) + +See `.github/workflows/main.yml` in this repo for example build+install on ubuntu + +### build + install xo-kalmanfilter +``` +$ cd xo-kalmanfilter +$ mkdir build +$ cd build +$ INSTALL_PREFIX=/usr/local # or wherever you prefer +$ cmake -DCMAKE_MODULE_PATH=${INSTALL_PREFIX}/share/cmake -DCMAKE_PREFIX_PATH=${INSTALL_PREFIX} -DCMAKE_INSTALL_PREFIX=${INSTALL_PREFIX} .. +$ make +$ make install +``` +(also see .github/workflows/main.yml) + +### build for unit test coverage +``` +$ cd xo-kalmanfilter +$ mkdir ccov +$ cd ccov +$ cmake -DCMAKE_MODULE_PATH=${INSTALL_PREFIX}/share/cmake -DCMAKE_PREFIX_PATH=${INSTALL_PREFIX} -DCODE_COVERAGE=ON -DCMAKE_BUILD_TYPE=Debug .. +``` + +## Development + +### LSP support + +LSP looks for compile commands in the root of the source tree; +cmake creates them in the root of its build directory. + +``` +$ cd xo-kalmanfilter +$ ln -s build/compile_commands.json +``` + +### display cmake variables + +- `-L` list variables +- `-A` include 'advanced' variables +- `-H` include help text + +``` +$ cd xo-kalmanfilter/build +$ cmake -LAH +``` diff --git a/xo-kalmanfilter/cmake/xo-bootstrap-macros.cmake b/xo-kalmanfilter/cmake/xo-bootstrap-macros.cmake new file mode 100644 index 00000000..aba31169 --- /dev/null +++ b/xo-kalmanfilter/cmake/xo-bootstrap-macros.cmake @@ -0,0 +1,35 @@ +# ---------------------------------------------------------------- +# for example: +# $ PREFIX=/usr/local # for example +# $ cmake -DCMAKE_MODULE_PATH=prefix -DCMAKE_INSTALL_PREFIX=$PREFIX -B .build +# +# will get +# CMAKE_MODULE_PATH +# from xo-cmake-config --cmake-module-path +# +# and expect .cmake macros in +# CMAKE_MODULE_PATH/xo_macros/xo_cxx.cmake +# ---------------------------------------------------------------- + +find_program(XO_CMAKE_CONFIG_EXECUTABLE NAMES xo-cmake-config REQUIRED) + +if ("${XO_CMAKE_CONFIG_EXECUTABLE}" STREQUAL "XO_CMAKE_CONFIG_EXECUTABLE-NOT_FOUND") + message(FATAL "could not find xo-cmake-config executable") +endif() + +message(STATUS "XO_CMAKE_CONFIG_EXECUTABLE=${XO_CMAKE_CONFIG_EXECUTABLE}") + +if (NOT XO_SUBMODULE_BUILD) + if (("${CMAKE_MODULE_PATH}" STREQUAL "") OR ("${CMAKE_MODULE_PATH}" STREQUAL prefix)) + # default to typical install location for xo-project-macros + execute_process(COMMAND ${XO_CMAKE_CONFIG_EXECUTABLE} --cmake-module-path OUTPUT_VARIABLE CMAKE_MODULE_PATH) + message(STATUS "CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") + endif() +endif() + +# needs to have been installed somewhere on CMAKE_MODULE_PATH, +# (e.g. from xo-cmake with the same value for CMAKE_INSTALL_PREFIX) +# +include(xo_macros/xo_cxx) + +xo_cxx_bootstrap_message() diff --git a/xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in b/xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in new file mode 100644 index 00000000..950e70c0 --- /dev/null +++ b/xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in @@ -0,0 +1,17 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +# note: changes to find_dependency() calls here +# must coordinate with xo_dependency() calls +# in xo-reactor/src/reactor/CMakeLists.txt +# +find_dependency(reactor) +find_dependency(Eigen3) +#find_dependency(reflect) +#find_dependency(webutil) +#find_dependency(printjson) +#find_dependency(callback) + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") +check_required_components("@PROJECT_NAME@") diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/EigenUtil.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/EigenUtil.hpp new file mode 100644 index 00000000..07a86d48 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/EigenUtil.hpp @@ -0,0 +1,29 @@ +/* file EigenUtil.hpp + * + * author: Roland Conybeare, Sep 2022 + */ + +#pragma once + +namespace xo { + namespace json { class PrintJson; } + + namespace eigen { + class EigenUtil { + public: + /* reflection for + * Eigen::VectorXd + * VectorXb (= Eigen::Array; by analogy with Eigen::VectorXd) + */ + static void reflect_eigen(); + + /* json printers for: + * Eigen::VectorXd + * Eigen::MatrixXd + */ + static void provide_json_printers(json::PrintJson * pjson); + }; /*EigenUtil*/ + } /*namespace eigen*/ +} /*namespace xo*/ + +/* end EigenUtil.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilter.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilter.hpp new file mode 100644 index 00000000..896b19ca --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilter.hpp @@ -0,0 +1,127 @@ +/* @file KalmanFilter.hpp */ + +#pragma once + +#include "KalmanFilterSpec.hpp" + +namespace xo { + namespace kalman { + /* Specification for an ordinary discrete linear kalman filter. + * + * The filter generates estimates for a process observed at a discrete + * set of times tk in {t0, t1, .., tn} + * + * At each time tk we have the following: + * + * 0. x(0) initial estimate at t(0) + * P(0) initial priors: error covariance matrix for x(0) + * + * 1. x_(k), [n x 1] vector: + * system state, denoted by vector. + * (state is not directly observable, + * filter will attempt to estimate it) + * + * 2. w_(k), [n x 1] vector + * Q(k), [n x n] matrix + * + * w_(k) denotes system noise, + * gaussian with covariance Q(k). + * noise w_(k) is not directly observable. + * + * 3. z(k), [m x 1] vector: + * + * observation vector for time tk + * + * 4. v_(k), [m x 1] vector + * R(k), [m x m] matrix + * + * v_(k) denotes observation errors, + * gaussian with covariance R(k). + * noise v_(k) is not directly observable. + * + * 5. F(k), [n x n] matrix + * state transition matrix + * model system evolves according to: + * + * x_(k+1) = F(x).x_(k) + w_(k) + * + * 6. observations z(k) depend on system state: + * + * z(k) = H(k).x_(k) + v_(k) + * + * 7. Kalman filter outputs: + * x(k), [n x 1] vector + * Q(k), [n x n] matrix + * + * x(k) is optimal estimate for system state x_(k) + * P(k) is covariance matrix specifying confidence intervals + * for pairs (x(k)[i], x(k)[j]) + * + * filter specification consists of: + * n, m, x(0), P(0), F(k), Q(k), H(k), R(k) + * The cardinality of observations z(k) can vary over time, + * so to be precise, m can vary with tk; write as m(k) + * + * More details: + * - avoid having to specify t(k) in advance; + * instead defer until observation available + * so t(k) can be taken from polling timestamp + */ + + /* encapsulate a (linear) kalman filter + * together with event publishing + */ + class KalmanFilter { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + using utc_nanos = xo::time::utc_nanos; + + public: + /* create filter with specification given by spec, and initial state s0 */ + explicit KalmanFilter(KalmanFilterSpec spec); + + uint32_t step_no() const { return state_ext_->step_no(); } + utc_nanos tm() const { return state_ext_->tm(); } + KalmanFilterSpec const & filter_spec() const { return filter_spec_; } + KalmanFilterStep const & step() const { return step_; } + rp const & state_ext() const { return state_ext_; } + + /* notify kalman filter with input for time t(k+1) = input_kp1.tkp1() + * Require: input.tkp1() >= .current_tm() + * Promise: + * - .tm() = input_kp1.tkp1() + * - .step_no() = old .step_no() + 1 + * - .filter_spec_k, .step_k, .state_k updated + * for observations in input_kp1 + */ + void notify_input(rp const & input_kp1); + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* specification for kalman filter; + * produces process/observation matrices on demand + */ + KalmanFilterSpec filter_spec_; + + /* filter step for most recent observation */ + KalmanFilterStep step_; + + /* filter state as of most recent observation; + * result of applying KalmanFilterEngine::step() to contents of .step + */ + rp state_ext_; + }; /*KalmanFilter*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilter const & x) { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilter.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterEngine.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterEngine.hpp new file mode 100644 index 00000000..5ac6ef9a --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterEngine.hpp @@ -0,0 +1,185 @@ +/* @file KalmanFilterEngine.hpp */ + +#pragma once + +#include "KalmanFilterStep.hpp" +#include "KalmanFilterState.hpp" +#include "KalmanFilterTransition.hpp" +#include "KalmanFilterObservable.hpp" +#include "KalmanFilterInput.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterEngine { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + using utc_nanos = xo::time::utc_nanos; + + public: + /* evolution of system state + account for system noise, + * before incorporating effect of observations z(k+1) + * x(k) --> x(k+1|k) + * P(k) --> P(k+1|k) + * + * tkp1. time t(k+1) assoc'd with step k+1 + * sk. filter state at time tk: + * sk.k = k step # (starts from 0) + * sk.tk = t(k) time t(k) assoc'd with step #k + * sk.x = x(k) estimated state at time tk + * sk.P = P(k) quality of state estimate x(k) + * (error covariance matrix) + * Fk. state transition: + * Fk.F = F(k) state transition matrix + * Fk.Q = Q(k) covariance matrix for system noise + * + * returns propagated state estimate for t(k+1): + * retval.k = k+1 + * retval.tk = t(k+1) = tkp1 + * retval.x = x(k+1|k) + * retval.P = P(k+1|k) + */ + static rp extrapolate(utc_nanos tkp1, + rp const & sk, + KalmanFilterTransition const & Fk); + + /* compute kalman gain matrix for filter step t(k) -> t(k+1) + * Expensive implementation using matrix inversion + * + * T + * M(k+1) = H(k).P(k+1|k).H(k) + R(k) + * + * T -1 + * K(k+1) = P(k+1|k).H(k) .M(k+1) + * + * Require: + * - skp1_ext.n_state() = Hkp1.n_state() + * + * skp1_ext. extrapolated filter state at t(k+1) + * Hkp1. relates model state to observable variables, + * for step t(k) -> t(k+1) + */ + static MatrixXd kalman_gain(rp const & skp1_ext, + KalmanFilterObservable const & Hkp1); + + /* compute kalman gain for a single observation z(k)[j]. + * This is useful iff the observation error matrix R is diagonal. + * For diagonal R we can present a set of observations z(k) serially + * instead of all at once, with lower time complexity + * + * Kalman Filter specifies some space with m observables. + * j identifies one of those observables, indexing from 0. + * This corresponds to row #j of H(k), and element R[j,j] of R. + * + * Effectively, we are projecting the kalman filter assoc'd with + * {skp1_ext, Hkp1} to a filter with a single observable variable z(k)[j], + * then computing the (scalar) kalman gain for this 1-variable filter + * + * The gain vector tells us for each member of filter state, + * how much to adjust our optimal estimate for that member for a unit + * amount of innovation in observable #j, i.e. for difference between + * expected and actual value for that observable. + */ + static VectorXd kalman_gain1(rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + uint32_t j); + + /* correct extrapolated state+cov estimate; + * also computes kalman gain + * + * Require: + * - skp1_ext.n_state() = Hkp1.n_state() + * - zkp1.n_obs() == Hkp1.n_observable() + * + * skp1_ext. extrapolated kalman state + covaraince at t(k+1) + * Hkp1. relates model state to observable variables + * for step t(k) -> t(k+1) + * zkp1. observations for time t(k+1) + * + * return new filter state+cov + */ + static rp correct(rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + rp const & zkp1); + + /* correct extrapolated filter state for observation + * of j'th filter observable z(k+1)[j] + * + * Can use this when observation errors are uncorrelated + * (i.e. observation error matrix R is diagonal) + */ + static rp correct1(rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + rp const & zkp1, + uint32_t j); + + /* step filter from t(k) -> t(k+1) + * + * sk. filter state from previous step: + * x (state vector), P (state covar matrix) + * Fk. transition-related params: + * F (transition matrix), Q (system noise covar matrix) + * Hkp1. observation-related params: + * H (coupling matrix), R (error covar matrix) + * zkp1. observations z(k+1) for time t(k+1) + */ + static rp step(utc_nanos tkp1, + rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + rp const & zkp1); + + /* step filter from t(k) -> tk(k+1) + * same as + * .step(tkp1, sk, step_spec.model(), step_spec.obs(), zkp1); + * + * step_spec. encapsulates Fk (transition-related params) + * and Q (system noise covar matrix) + */ + static rp step(KalmanFilterStep const & step_spec); + + /* step filter from t(k) -> t(k+1) + * + * sk. filter state from previous step: + * x (state vector), P (state covar matrix) + * Fk. transition-related params: + * F (transition matrix), Q (system noise covar matrix) + * Hkp1. observation-related params: + * H (coupling matrix), R (error covar matrix) + * zkp1. observations z(k+1) for time t(k+1) + * j. identifies a single filter observable -- + * step will only consume observation z(k+1)[j] + */ + static rp step1(utc_nanos tkp1, + rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + rp const & zkp1, + uint32_t j); + + /* step filter from t(k) -> t(k+1) + * + * same as + * .step1(step_spec.tkp1(), + * step_spec.state(), + * step_spec.model(), + * step_spec.obs(), + * step_spec.input(), + * j); + * + * step_spec. encapsulates + * x (state vector), + * P (state covar matrix), + * Fk (transition-related params), + * Q (system noise covar matrix) + * z (z(k+1), observations at time t(k+1)) + * j. identifies a single filter observable -- + * step will only consume observation z(k+1)[j] + */ + static rp step1(KalmanFilterStep const & step_spec, + uint32_t j); + }; /*KalmanFilterEngine*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInput.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInput.hpp new file mode 100644 index 00000000..9319ce58 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInput.hpp @@ -0,0 +1,121 @@ +/* @file KalmanFilterInput.hpp */ + +#pragma once + +#include "xo/reflect/SelfTagging.hpp" +//#include "time/Time.hpp" +//#include "xo/refcnt/Refcounted.hpp" +#include +#include + +namespace xo { + /* FIXME. hack here to get member access working for reflection */ + namespace vf { class StrikesetKfinput; } + + namespace kalman { + /* represents a single kalman filter input event + * comprising: + * - time tkp1 + * - observation vector z[] + * - presence bits presence[] for z + * - observation errors Rd[] + */ + class KalmanFilterInput : public reflect::SelfTagging { + public: + using TaggedRcptr = xo::reflect::TaggedRcptr; + using utc_nanos = xo::time::utc_nanos; + using VectorXb = Eigen::Array; + using VectorXi = Eigen::VectorXi; + using VectorXd = Eigen::VectorXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterInput() = default; + + static rp make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd); + + /* create input, with all presence bits set + not using Rd */ + static rp make_present(utc_nanos tkp1, + VectorXd const & z); + + /* reflect KalmanFilterInput object representation */ + static void reflect_self(); + + /* alt name -- concession to reactor::DirectSource + * when event type is KalmanFilterInput + */ + utc_nanos tm() const { return tkp1_; } + + utc_nanos tkp1() const { return tkp1_; } + uint32_t n_obs() const { return z_.size(); } + VectorXb const & presence() const { return presence_; } + VectorXd const & z() const { return z_; } + VectorXd const & Rd() const { return Rd_; } + + /* computes reindexer keep[]: + * .presence[keep[j*]] + * is the j*'th true value in .presence + */ + VectorXi make_kept_index() const; + + virtual void display(std::ostream & os) const; + std::string display_string() const; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + protected: + KalmanFilterInput(utc_nanos tkp1, VectorXb presence, VectorXd z, VectorXd Rd) + : tkp1_(tkp1), + presence_{std::move(presence)}, + z_{std::move(z)}, + Rd_{std::move(Rd)} {} + + friend class xo::vf::StrikesetKfinput; + + private: + /* t(k+1) - asof time for observations .z */ + utc_nanos tkp1_ = xo::time::timeutil::epoch(); + /* [m x 1] presence vector. + * an observation z[j] is present iff .presence[j] is true. + * rows/columns for an absent observation are removed from filter matrices + */ + VectorXb presence_; + /* [m x 1] observation vector z(k) */ + VectorXd z_; + + /* [m x 1] observation error vector Rd(k). + * This represents a side channel for passing the diagonal of + * observation matrix R(k), when both: + * (a) error of different observations are assumed to be uncorrelated (likely) + * (b) error variance is derived from input data, e.g. because of + * some input preprocessing. + * + * It's up to KalmanFilterSpec::MkStepFn to opt-in to using this information, + * which requires agreement with any input preparation step. + * + * This variable could just as well provide observation error matrix R + * + * NOTE: perhaps-cleaner alternative would be to inherit KalmanFilterInput to + * introduce additional state, then MkStepFn can dynamic_cast + */ + VectorXd Rd_; + }; /*KalmanFilterInput*/ + + using KalmanFilterInputPtr = rp; + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInput const & x) + { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp new file mode 100644 index 00000000..d89c6b68 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp @@ -0,0 +1,14 @@ +/* @file KalmanFilterInputCallback.hpp */ + +#pragma once + +#include "xo/refcnt/Refcounted.hpp" +#include "KalmanFilter.hpp" + +namespace xo { + namespace kalman { + using KalmanFilterInputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputCallback.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputSource.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputSource.hpp new file mode 100644 index 00000000..7e801936 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputSource.hpp @@ -0,0 +1,27 @@ +/* @file KalmanFilterInputSource.hpp */ + +#pragma once + +#include "xo/reactor/EventSource.hpp" +#include "KalmanFilterInputCallback.hpp" + +namespace xo { + namespace kalman { + /* Use: + * rp src = ...; + * rp in_cb = ...; + * + * src->add_callback(in_cb); + * ... + * // src invokes in_cb->notify_input( + * src->remove_callback(in_cb); + */ + using KalmanFilterInputSource = reactor::EventSource< + /*KalmanFilterInput,*/ + KalmanFilterInputCallback + /*&KalmanFilterInputCallback::notify_filter*/ + >; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputSource.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp new file mode 100644 index 00000000..bdc00e10 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterInputToConsole.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilterInput.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterInputToConsole + : public xo::reactor::SinkToConsole> + { + public: + KalmanFilterInputToConsole() = default; + + static rp make(); + + virtual void display(std::ostream & os) const; + //virtual std::string display_string() const; + }; /*KalmanFilterInputToConsole*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInputToConsole const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace option*/ +} /*namespace xo*/ + +/* end KalmanFilterInputToConsole.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterObservable.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterObservable.hpp new file mode 100644 index 00000000..c3981095 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterObservable.hpp @@ -0,0 +1,109 @@ +/* @file KalmanFilterObservable.hpp */ + +#pragma once + +//#include "time/Time.hpp" +#include +#include + +namespace xo { + namespace kalman { + class KalmanFilterObservable { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXi = Eigen::VectorXi; + + public: + KalmanFilterObservable() = default; + + /* keep maps back to canonical observations z(j). + * H, R have been re-indexed + * + * If all m observations are included, then keep will be: + * [0, .., m-1] + */ + KalmanFilterObservable(VectorXi keep, MatrixXd H, MatrixXd R) + : keep_{std::move(keep)}, H_{std::move(H)}, R_{std::move(R)} { + assert(this->check_ok()); + } /*ctor*/ + + /* build KF observable object where keeping all observations */ + static KalmanFilterObservable keep_all(MatrixXd H, MatrixXd R); + + /* build KF observable object. replace H, R with reindexed versions H', R' + * according to indexing vector keep[]. keep[] indexes members of + * observation vector z(k)[j]. Reindexed z', H', R' as follows: + * + * z'[j*] = z[keep[j*]] + * H'[j*, i] = H[keep[j*], i] + * R'[j1*, j2*] = R[keep[j1*], keep[j2*]] + */ + static KalmanFilterObservable reindex(VectorXi keep, MatrixXd H, MatrixXd R); + + uint32_t n_state() const { return H_.cols(); } + uint32_t n_observable() const { return H_.rows(); } + VectorXi const & keep() const { return keep_; } + MatrixXd const & observable() const { return H_; } + MatrixXd const & observable_cov() const { return R_; } + + bool check_ok() const { + uint32_t m = H_.rows(); + bool keep_is_mx1 = (keep_.rows() == m); + bool r_is_mxm = ((R_.cols() == m) && (R_.rows() == m)); + + bool keep_is_well_ordered = true; + + /* members of .keep are non-negative integers, + * in strictly increasing order + */ + int64_t keep_jm1 = -1; + + for (uint32_t j = 0; j < keep_.rows(); ++j) { + if (keep_[j] < 0) + keep_is_well_ordered = false; + if (keep_[j] <= keep_jm1) + keep_is_well_ordered = false; + } + + /* also would like to require: R is +ve definite */ + + return keep_is_mx1 && keep_is_well_ordered && r_is_mxm; + } /*check_ok*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + + private: + /* m: #of observations that survived sanity/error checks + * + * H, R here will have been re-indexed to exclude rejected observations. + * observations z will also have been re-indexed. + * + * If an observation z[j] is excluded, then also exclude: + * - j'th row of H + * - j'th row and j'th column of R + * - j'th element of z + * + * Given re-indexed H, R, the j*'th row of H goes with z[keep[j*]] + */ + + /* [m x 1] maps back to indices in original observation vector */ + VectorXi keep_; + /* [m x n] observation matrix */ + MatrixXd H_; + /* [m x m] covariance matrix for observation noise */ + MatrixXd R_; + }; /*KalmanFilterObservable*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterObservable const & x) + { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterObservable.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp new file mode 100644 index 00000000..69cfb4bc --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp @@ -0,0 +1,15 @@ +/* @file KalmanFilterOutputCallback.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilter.hpp" + +namespace xo { + namespace kalman { + /* callback for consuming kalman filter output */ + using KalmanFilterOutputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterOutputCallback.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSpec.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSpec.hpp new file mode 100644 index 00000000..3bcca6ad --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSpec.hpp @@ -0,0 +1,86 @@ +/* @file KalmanFilterSpec.hpp */ + +#pragma once + +#include "KalmanFilterStep.hpp" + +namespace xo { + namespace kalman { + /* full specification for a kalman filter. + * + * For a textbook linear filter, expect a KalmanFilterStep + * instance to be independent of KalmanFilterState/KalmanFilterInput. + * + * Relaxing this requirement for two reasons: + * 1. (proper) want to allow filter with variable timing between observations, + * expecially if observations are event-driven. + * In that case it's likely that state transition matrices are a function + * of elapsed time between observations. Providing filter state sk + * allows MkStepFn to use sk.tm() + * 2. (sketchy) when observations represent market data, + * desirable to treat an observation as giving one-sided information + * about true value. For example treat a bid price as evidence + * true value is higher than that bid, but don't want to constrain + * "how much higher". Certainly no reason to think that + * bid price is normally distributed around fair value. + * Allow for hack in which we + * and modulate error distribution "as if it were normal" to assess + * a non-gaussian error distribution + */ + class KalmanFilterSpec { + public: + /* typical implementation will look something like: + * mk_step(KalmanFilterState const & sk, + * KalmanFilterInputPtr const & zkp1) + * { + * KalmanFilterTransition model = ...; + * KalmanFilterObservable obs = ...; + * + * return KalmanFilterStep(sk, model, obs, zkp1); + * } + */ + using MkStepFn = std::function const & sk, + KalmanFilterInputPtr const & zkp1)>; + + public: + explicit KalmanFilterSpec(rp s0, MkStepFn mkstepfn) + : start_ext_{std::move(s0)}, + mk_step_fn_{std::move(mkstepfn)} {} + + rp const & start_ext() const { return start_ext_; } + /* get step parameters (i.e. matrices F, Q, H, R) + * for step t(k) -> t(k+1). + * + * We supply t(k) state s and t(k+1) observation z(k+1): + * - to allow time stepping to be observation-driven + * - to allow for selective outlier removal + */ + KalmanFilterStep make_step(rp const & sk, + rp const & zkp1) { + return this->mk_step_fn_(sk, zkp1); + } /*make_step*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* starting state */ + rp start_ext_; + + /* creates kalman filter step object on demand; + * step object specifies inputs to 1 step in discrete + * linear kalman filter + */ + MkStepFn mk_step_fn_; + }; /*KalmanFilterSpec*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterSpec const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterState.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterState.hpp new file mode 100644 index 00000000..fe814b9a --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterState.hpp @@ -0,0 +1,163 @@ +/* @file KalmanFilterState.hpp */ + +#pragma once + +#include "xo/reflect/SelfTagging.hpp" +#include "KalmanFilterInput.hpp" +#include "KalmanFilterTransition.hpp" +//#include "time/Time.hpp" +#include +#include +#include + +namespace xo { + namespace kalman { + /* encapsulate state (i.e. initial state, and output after each step) + * for a kalman filter + */ + class KalmanFilterState : public reflect::SelfTagging { + public: + using TaggedRcptr = reflect::TaggedRcptr; + using utc_nanos = xo::time::utc_nanos; + using VectorXd = Eigen::VectorXd; + using MatrixXd = Eigen::MatrixXd; + using uint32_t = std::uint32_t; + + public: + static rp make(); + static rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition); + virtual ~KalmanFilterState() = default; + + /* reflect KalmanFilterState object representation */ + static void reflect_self(); + + uint32_t step_no() const { return k_; } + utc_nanos tm() const { return tk_; } + /* with n = .n_state(): + * x_ is [n x 1] vector + * P_ is [n x n] matrix, + */ + uint32_t n_state() const { return x_.size(); } + VectorXd const & state_v() const { return x_; } + MatrixXd const & state_cov() const { return P_; } + + KalmanFilterTransition const & transition() const { return transition_; } + + virtual void display(std::ostream & os) const; + std::string display_string() const; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + private: + KalmanFilterState(); + KalmanFilterState(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition); + + friend class KalmanFilterStateExt; + + private: + /* step# k, advances by +1 on each filter step */ + uint32_t k_ = 0; + /* time t(k) */ + utc_nanos tk_; + /* [n x 1] (estimated) system state xk = x(k) */ + VectorXd x_; + /* [n x n] covariance matrix for error assoc'd with with x(k) + * P(i,j) is the covariance of (ek[i], ek[j]), + * where ex(k) is the difference (x(k) - x_(k)) + * between estimated state x(k) + * (= this->x_) and model state x_(k) + */ + MatrixXd P_; + + /* F, Q matrices driving .x, .P */ + KalmanFilterTransition transition_; + }; /*KalmanFilterState*/ + + inline std::ostream & operator<<(std::ostream & os, + KalmanFilterState const & s) + { + s.display(os); + return os; + } /*operator<<*/ + + /* KalmanFilterStateExt: + * adds additional details from filter step to KalmanFilterState + */ + class KalmanFilterStateExt : public KalmanFilterState { + public: + using TaggedRcptr = reflect::TaggedRcptr; + using MatrixXd = Eigen::MatrixXd; + using int32_t = std::int32_t; + + public: + static rp make(); + static rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk); + + /* create state object for initial filter state */ + static rp initial(utc_nanos t0, + VectorXd x0, + MatrixXd P0); + + /* reflect KalmanFilterStateExt object representation */ + static void reflect_self(); + + int32_t observable() const { return j_; } + MatrixXd const & gain() const { return K_; } + rp const & zk() const { return zk_; } + + virtual void display(std::ostream & os) const override; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + private: + KalmanFilterStateExt() = default; + KalmanFilterStateExt(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk); + + private: + /* if -1: not used; + * if >= 0: identifies j'th of m observables; + * gain .K applies just to information obtainable from + * observing that scalar variable + */ + int32_t j_ = -1; + /* if .j is -1: + * [n x n] kalman gain + * if .j >= 0: + * [n x 1] kalman gain for observable #j + */ + MatrixXd K_; + /* input leading to state k. + * empty for initial state (i.e. when .k is 0) + */ + rp zk_; + }; /*KalamnFilterStateExt*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp new file mode 100644 index 00000000..4f86b574 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterStateToConsole.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilterState.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterStateToConsole + : public xo::reactor::SinkToConsole + { + public: + KalmanFilterStateToConsole() = default; + + static rp make(); + + virtual void display(std::ostream & os) const; + //virtual std::string display_string() const; + }; /*KalmanFilterStateToConsole*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterStateToConsole const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace option*/ +} /*namespace xo*/ + +/* end KalmanFilterStateToConsole.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStep.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStep.hpp new file mode 100644 index 00000000..781b63a6 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStep.hpp @@ -0,0 +1,128 @@ +/* @file KalmanFilterStep.hpp */ + +#pragma once + +#include "KalmanFilterState.hpp" +#include "KalmanFilterInput.hpp" +#include "KalmanFilterTransition.hpp" +#include "KalmanFilterObservable.hpp" + +namespace xo { + namespace kalman { + /* encapsulate {state + observation} models for a single time step t(k). + * Emitted by KalmanFilterSpec, q.v. + */ + class KalmanFilterStepBase { + public: + KalmanFilterStepBase() = default; + KalmanFilterStepBase(KalmanFilterTransition model, + KalmanFilterObservable obs) + : model_{std::move(model)}, + obs_{std::move(obs)} {} + + /* aka system_model() */ + KalmanFilterTransition const & model() const { return model_; } + KalmanFilterObservable const & obs() const { return obs_; } + + private: + /* model for process being observed (state transition + noise) */ + KalmanFilterTransition model_; + /* what can be observed (observables + noise) */ + KalmanFilterObservable obs_; + }; /*KalmanFilterStepBase*/ + + /* encapsulate {state + observation} models for a single time step t(k). + * Emitted by KalmanFilterSpec, q.v. + * + * holds: + * x(k) + * P(k) + * F(k) + * H(k+1) + * z(k+1) + * + * contains all the inputs needed to compute: + * x(k+1) + * P(k+1) + * + * does not provide that result + */ + class KalmanFilterStep : public KalmanFilterStepBase { + public: + using utc_nanos = xo::time::utc_nanos; + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + + public: + KalmanFilterStep() = default; + KalmanFilterStep(rp state, + KalmanFilterTransition model, + KalmanFilterObservable obs, + rp zkp1) + : KalmanFilterStepBase(model, obs), + state_{std::move(state)}, + input_{std::move(zkp1)} {} + + rp const & state() const { return state_; } + rp const & input() const { return input_; } + + utc_nanos tkp1() const { return input_->tkp1(); } + + /* extrapolate kalman filter state forward to time + * .tkp1() (i.e. to t(k+1)); computes + * x(k+1|k) + * P(k+1|k) + * does not use the t(k+1) observations .input.z + */ + rp extrapolate() const; + + /* compute kalman gain matrix K(k+1) + * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)} + * + * note that .state() != skp1_ext; .state() reports {x(k), P(k)} + */ + MatrixXd gain(rp const & skp1_ext) const; + + /* compute kalman gain vector K(k+1) + * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)}, + * on behalf of a single observation z[j]. + * actual observation z[j] is not given here, + * just computing the gain vector. i'th member of gain vector + * gives effect of innovation on i'th member of kalman filter state. + */ + VectorXd gain1(rp const & skp1_ext, + uint32_t j) const; + + /* compute correction to extrapolated filter state {x(k+1|k), P(k+1|k)}, + * for observation z(k+1) = .input.z() + */ + rp correct(rp const & skp1_ext); + + /* compute correction to extrapolated filter state skp1_ext = {x(k+1|k), P(k+1|k)}, + * for a single observation z(k+1, j) = .input.z()[j] + */ + rp correct1(rp const & skp1_ext, + uint32_t j); + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* system state: timestamp, estimated process state, process covariance + * asof beginning of this step + */ + rp state_; + /* input: observations at time t(k+1) */ + KalmanFilterInputPtr input_; + }; /*KalmanFilterStep*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterStep const & x) { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStep.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSvc.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSvc.hpp new file mode 100644 index 00000000..4b63cbee --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSvc.hpp @@ -0,0 +1,64 @@ +/* @file KalmanFilterSvc.hpp */ + +#include "xo/reactor/Sink.hpp" +#include "xo/reactor/DirectSourcePtr.hpp" +#include "KalmanFilter.hpp" +#include "KalmanFilterInputSource.hpp" +#include "KalmanFilterOutputCallback.hpp" +#include "xo/callback/CallbackSet.hpp" + +namespace xo { + namespace kalman { + /* encapsulate a passive KalmanFilter + * instance as an active event consumer+producer + * + * sinks that want to consume KalmanFilterSvc events will use + * .attach_sink() (or .add_callback()) + */ + class KalmanFilterSvc : public xo::reactor::Sink1>, + public xo::reactor::DirectSourcePtr> { + public: + using AbstractSource = xo::reactor::AbstractSource; + + public: + /* named ctor idiom */ + static rp make(KalmanFilterSpec spec); + + KalmanFilter const & filter() const { return filter_; } + + /* notify incoming observations; will trigger kalman filter step */ + void notify_ev(rp const & input_kp1) override; + + // ----- inherited from reactor::AbstractSink ----- + + /* filter captures KF input pointer */ + virtual bool allow_volatile_source() const override { return false; } + virtual uint32_t n_in_ev() const override { return n_in_ev_; } + virtual void display(std::ostream & os) const override; + + // ----- inherited from reactor::AbstractSource ----- + + /* note: correct since KalmanFilterEngine.extrapolate() + * always creates new state object + */ + virtual bool is_volatile() const override { return false; } + + // ----- Inherited from AbstractEventProcessor ----- + + private: + KalmanFilterSvc(KalmanFilterSpec spec); + + private: + /* passive kalman filter */ + KalmanFilter filter_; + /* receive filter input from this source; see .attach_input() */ + rp input_src_; + /* counts lifetime #of input events (see .notify_ev()) */ + uint32_t n_in_ev_ = 0; + /* publish filter state updates to these callbacks */ + fn::RpCallbackSet pub_; + }; /*KalmanFilterSvc*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* KalmanFilterSvc.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterTransition.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterTransition.hpp new file mode 100644 index 00000000..22d3de5b --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterTransition.hpp @@ -0,0 +1,62 @@ +/* @file KalmanFilterTransition.hpp */ + +#pragma once + +//#include "time/Time.hpp" +#include +#include + +namespace xo { + namespace kalman { + + /* encapsulate transition behavior for a kalman filter + * before taking observations into account + */ + class KalmanFilterTransition { + public: + using MatrixXd = Eigen::MatrixXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterTransition() = default; + KalmanFilterTransition(MatrixXd F, + MatrixXd Q) + : F_{std::move(F)}, Q_{std::move(Q)} { assert(this->check_ok()); } + + static void reflect_self(); + + /* n: cardinality of state vector */ + uint32_t n_state() const; + + MatrixXd const & transition_mat() const { return F_; } + MatrixXd const & transition_cov() const { return Q_; } + + bool check_ok() const { + uint32_t n = F_.rows(); + bool f_is_nxn = ((F_.rows() == n) && (F_.cols() == n)); + bool q_is_nxn = ((Q_.rows() == n) && (Q_.cols() == n)); + + /* also would like to require: Q is +ve definite */ + + return f_is_nxn && q_is_nxn; + } /*check_ok*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* [n x n] state transition matrix */ + MatrixXd F_; + /* [n x n] covariance matrix for system noise */ + MatrixXd Q_; + }; /*KalmanFilterTransition*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterTransition const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterTransition.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/init_filter.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/init_filter.hpp new file mode 100644 index 00000000..292a63ff --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/init_filter.hpp @@ -0,0 +1,20 @@ +/* file init_kalmanfilter.hpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#pragma once + +#include "xo/subsys/Subsystem.hpp" + +namespace xo { + enum S_kalmanfilter_tag {}; + + template<> + struct InitSubsys { + static void init(); + static InitEvidence require(); + }; +} /*namespace xo*/ + +/* end init_kalmanfilter.hpp */ diff --git a/xo-kalmanfilter/include/xo/kalmanfilter/print_eigen.hpp b/xo-kalmanfilter/include/xo/kalmanfilter/print_eigen.hpp new file mode 100644 index 00000000..ea6f7375 --- /dev/null +++ b/xo-kalmanfilter/include/xo/kalmanfilter/print_eigen.hpp @@ -0,0 +1,41 @@ +/* @file print_eigen.hpp */ + +#include +#include + +namespace logutil { + template + class matrix { + public: + matrix(T x) : x_{std::move(x)} {} + + /* print this value */ + T x_; + }; /*matrix*/ + + template + using vector = matrix; + + template + inline std::ostream & + operator<<(std::ostream & s, matrix const & mat) + { + s << "["; + for(std::uint32_t i = 0, m = mat.x_.rows(); i 0) + s << "; "; + + for(std::uint32_t j = 0, n = mat.x_.cols(); j 0) + s << ' '; + + s << mat.x_(i, j); + } + } + s << "]"; + + return s; + } /*operator<<*/ +} /*namespace logutil*/ + +/* end print_eigen.hpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt b/xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt new file mode 100644 index 00000000..882db194 --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt @@ -0,0 +1,30 @@ +# xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt + +set(SELF_LIB xo_kalmanfilter) + +set(SELF_SRCS + EigenUtil.cpp + KalmanFilterInput.cpp + KalmanFilterInputToConsole.cpp + KalmanFilterState.cpp + KalmanFilterStateToConsole.cpp + KalmanFilterTransition.cpp + KalmanFilterObservable.cpp + KalmanFilterEngine.cpp + KalmanFilter.cpp + KalmanFilterStep.cpp + KalmanFilterSpec.cpp + KalmanFilterSvc.cpp + init_filter.cpp +) + +xo_add_shared_library4(${SELF_LIB} ${PROJECT_NAME}Targets ${PROJECT_VERSION} 1 ${SELF_SRCS}) + +# ---------------------------------------------------------------- +# Dependencies +# +# REMINDER: must coordinate with find_dependency() calls in +# [xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in] + +xo_dependency(${SELF_LIB} reactor) +xo_external_target_dependency(${SELF_LIB} Eigen3 Eigen3::Eigen) diff --git a/xo-kalmanfilter/src/kalmanfilter/EigenUtil.cpp b/xo-kalmanfilter/src/kalmanfilter/EigenUtil.cpp new file mode 100644 index 00000000..b5770440 --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/EigenUtil.cpp @@ -0,0 +1,157 @@ +/* file EigenUtil.cpp + * + * author: Roland Conybeare, Sep 2022 + */ + +#include "EigenUtil.hpp" +#include "xo/printjson/PrintJson.hpp" +#include "xo/reflect/Reflect.hpp" +#include +#include +#include +#include + +namespace xo { + using xo::json::PrintJson; + using xo::json::JsonPrinter; + using xo::reflect::Reflect; + using xo::reflect::TypeDescr; + using VectorXb = Eigen::Array; + using Eigen::VectorXd; + using Eigen::MatrixXd; + +#ifdef NOT_YET + namespace reflect { + template + using EigenVectorX_Tdx = xo::reflect::StlVectorTdx>; + + /* probably need this to appear before decl for class xo::reflect::Reflect */ + template + class EstablishTdx> { + public: + static std::unique_ptr make() { + return EigenVectorX_Tdx::make(); + } /*make*/ + }; /*EstablishTdx*/ + } /*reflect*/ +#endif + + namespace eigen { + + namespace { + /* prints a VectorXd as json, in the obvious format, e.g. + * [1,2,3] + */ + template + class EigenVectorJsonPrinter : public JsonPrinter { + public: + EigenVectorJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} + + virtual void print_json(TaggedPtr tp, + std::ostream * p_os) const override + { + EigenVectorType * pv = this->check_recover_native(tp, p_os); + + if (pv) { + /* EigenVectorType (VectorXb, VectorXd, ..) + * is reflected as atomic for now, out of expedience. + * + * as soon as we reflect as mt_vector, will not need this helper. + */ + *p_os << "["; + + for (std::uint32_t i = 0, n = pv->size(); i < n; ++i) { + if (i > 0) + *p_os << ","; + + /* note: need to dispatch via json printer for vector elements, + * to get special treatment for non-finite values + */ + this->pjson()->print((*pv)[i], p_os); + //*p_os << jsonp((*pv)[i], this->pjson()); + } + + *p_os << "]"; + } + } /*print_json*/ + }; /*EigenVectorJsonPrinter*/ + + /* prints a MatrixXd as json, in row-major format, e.g. + * [[1,2,3], [4,5,6], [7,8,9]] + */ + class MatrixXdJsonPrinter : public JsonPrinter { + public: + MatrixXdJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} + + virtual void print_json(TaggedPtr tp, + std::ostream * p_os) const override + { + MatrixXd * pm = this->check_recover_native(tp, p_os); + + if (pm) { + /* MatrixXd is reflected as atomic for now, out of expedience */ + *p_os << "["; + + for(std::uint32_t i=0, m=pm->rows(); i 0) + *p_os << ", "; + *p_os << "["; + for(std::uint32_t j=0, n=pm->cols(); j 0) + *p_os << ","; + + /* note: need to dispatch via json printer for matrix elements, + * to get special treatment for non-finite values + */ + this->pjson()->print((*pm)(i, j), p_os); + //*p_os << jsonp((*pm)(i, j), this->pjson()); + } + *p_os << "]"; + } + + *p_os << "]"; + } + } /*print_json*/ + }; /*MatrixXdJsonPrinter*/ + + template + void + provide_eigen_vector_printer(PrintJson * p_pjson) + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new EigenVectorJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } /*provide_eigen_vector_printer*/ + } /*namespace*/ + + void + EigenUtil::reflect_eigen() + { +#ifdef NOT_YET + Reflect::require(); + Reflect::require(); +#endif + } /*reflect_eigen*/ + + void + EigenUtil::provide_json_printers(PrintJson * p_pjson) + { + assert(p_pjson); + + provide_eigen_vector_printer(p_pjson); + provide_eigen_vector_printer(p_pjson); + + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new MatrixXdJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } + } /*provide_json_printers*/ + } /*namespace eigen*/ +} /*namespace xo*/ + +/* end EigenUtil.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilter.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilter.cpp new file mode 100644 index 00000000..5c415580 --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilter.cpp @@ -0,0 +1,78 @@ +/* @file KalmanFilter.cpp */ + +#include "KalmanFilter.hpp" +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" + +namespace xo { + using xo::time::utc_nanos; + //using logutil::matrix; + using xo::scope; + using xo::tostr; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilter ----- + + KalmanFilter::KalmanFilter(KalmanFilterSpec spec) + : filter_spec_{std::move(spec)}, + state_ext_{filter_spec_.start_ext()} + {} /*ctor*/ + + void + KalmanFilter::notify_input(rp const & input_kp1) + { + scope log(XO_ENTER0(info)); + + /* on entry: + * .state_ext refers to t(k) + * on exit: + * .step refers to t(k+1) + * .state_ext refers to t(k+1) + */ + + log && log(xtag("step_dt", + input_kp1->tkp1() - this->state_ext_->tm())); + + /* establish step inputs for this filter step: + * F(k+1) (system transition matrix) + * Q(k+1) (system noise covariance matrix) + * H(k+1) (observation coupling matrix) + * R(k+1) (observation noise covariance matrix) + * z(k+1) (observation vector) + */ + this->step_ = this->filter_spec_.make_step(this->state_ext_, input_kp1); + + //if (lscope.enabled()) { lscope.log(xtag("step", this->step_)); } + + /* extrapolate filter state to t(k+1), + * and correct based on z(k+1) + */ + this->state_ext_ = KalmanFilterEngine::step(this->step_); + + //if (lscope.enabled()) { lscope.log(xtag("state_ext", this->state_ext_)); } + } /*notify_input*/ + + void + KalmanFilter::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilter::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilter.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterEngine.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterEngine.cpp new file mode 100644 index 00000000..fe783e8a --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterEngine.cpp @@ -0,0 +1,360 @@ +/* @file KalmanFilterEngine.cpp + * + */ + +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" + +namespace xo { + using xo::time::utc_nanos; + using logutil::matrix; + using xo::scope; + using xo::xtag; + using Eigen::LDLT; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilterEngine ----- + + rp + KalmanFilterEngine::extrapolate(utc_nanos tkp1, + rp const & s, + KalmanFilterTransition const & f) + { + //constexpr char const * c_self_name + // = "KalmanFilterEngine::extrapolate"; + + /* prior estimates at t(k) */ + VectorXd const & x = s->state_v(); + MatrixXd const & P = s->state_cov(); + + /* model change from t(k) -> t(k+1) */ + MatrixXd const & F = f.transition_mat(); + MatrixXd const & Q = f.transition_cov(); + + if(F.cols() != x.rows()) { + scope log(XO_DEBUG(true /*debug_flag*/)); + + log("error: F*x: expected F.cols=x.rows", + xtag("F.cols", F.cols()), xtag("x.rows", x.rows())); + } + + /* x(k+1|k) */ + VectorXd x_ext = F * x; + + /* P(k+1|k) */ + MatrixXd P_ext = (F * P * F.transpose()) + Q; + + /* creating new state object here + * allows KalmanFilterSvc.is_volatile()=false + */ + + return KalmanFilterState::make(s->step_no() + 1, + tkp1, + std::move(x_ext), + std::move(P_ext), + f); + } /*extrapolate*/ + + VectorXd + KalmanFilterEngine::kalman_gain1(rp const & skp1_ext, + KalmanFilterObservable const & h, + uint32_t j) + { + constexpr bool c_debug_enabled = false; + + scope log(XO_DEBUG(c_debug_enabled)); + + /* P(k+1|k) :: [n x n] */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* H(k) :: [m x n] */ + MatrixXd const & H = h.observable(); + /* R(k) :: [m x m] */ + MatrixXd const & R = h.observable_cov(); + + /* i'th col of H couples element #i of filter state to each member of input z(k); + * j'th row of H couples filter state to j'th observable + * + * Hj :: [1 x n] Hj is a row-vector + */ + auto Hj = H.row(j); + + /* Rjj is the j'th diagonal element of R */ + double Rjj = R(j, j); + + /* T + * M(k) = Hj * P(k+1|k) * Hj + Rjj + * + * M(k) is a [1 x 1] matrix + */ + double m = Hj * (P_ext * Hj.transpose()) + Rjj; + + /* -1 + * M(k) trivial, since M is [1 x 1] + */ + double m_inv = 1.0 / m; + + /* K :: [n x 1] */ + VectorXd K = P_ext * Hj.transpose() * m_inv; + + log && log("result", + xtag("P(k+1|k)", matrix(P_ext)), + xtag("R", matrix(R)), + xtag("m", m)); + + return K; + } /*kalman_gain1*/ + + MatrixXd + KalmanFilterEngine::kalman_gain(rp const & skp1_ext, + KalmanFilterObservable const & h) + { + scope log(XO_DEBUG(false /*debug_enabled*/)); + + /* P(k+1|k) */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + MatrixXd const & H = h.observable(); + MatrixXd const & R = h.observable_cov(); + + uint32_t m = H.rows(); + uint32_t n = H.cols(); + + if ((P_ext.rows() != n) || (P_ext.cols() != n)) { + std::string err_msg + = tostr("kalman_gain: with dim(H) = [m x n] expect dim(P) = [n x n]", + xtag("m", m), xtag("n", n), + xtag("P.rows", P_ext.rows()), + xtag("P.cols", P_ext.cols())); + + throw std::runtime_error(err_msg); + } + + if ((R.rows() != m) || (R.cols() != m)) { + std::string err_msg + = tostr("kalman_gain: with dim(H) = [m x n] expect dim(R) = [m x m]", + xtag("m", m), xtag("n", n), + xtag("R.rows", R.rows()), xtag("R.cols", R.cols())); + + throw std::runtime_error(err_msg); + } + + /* kalman gain: + * T -1 + * K(k+1) = P(k+1|k).H(k) .M + * + * T / T \ -1 + * = P(k+1|k).H(k) .| H(k).P(k+1|k).H(k) + R(k) | + * \ / + * + * Notes: + * 1. the matrix M being inverted is symmetric, since represents covariances. + * 2. if diagonal of R(k) has no zeroes (i.e. all measurements are subject to error), + * then it must be non-negative definite + * 3. unless observation errors are perfectly correlated, M(k) + * is positive definite. + * 4. even though 3. holds, there may be a nearby non-positive-definite matrix M+dM, + * Factoring M with finite-precision arithmetic solves for M+dM instead of M; + * which may run into difficulty if M is only 'slighlty' +ve definite. + * If necessary add small diagonal correction D to M, + * sufficient to make M+D positive definite. + * This is equivalent to introducing additional + * uncorrelated observation error, so benign from a robustness perspective + * 5. In generally we usually want to avoid fully realizing a matrix inverse. + * In this case need to explicitly compute K as ingredient used to + * correct state covariance later. + * 6. However, if R is diagonal (which is in practice quite likely), + * then it's easy to decompose a suite of vector observations z(k+1) = [z1, ..zm]T + * into separate zi, with dt=0 separating them. + * Can use this to avoid computing the inverse. + * See .kalman_gain1(), .correct1() + * 7. .kalman_gain() works unaltered when H, R have been reindexed + * to exclude outliers/errors; this is true because .kalman_gain() does not + * use the observation vector z[], i.e. operates entirely in the reduced + * reindexed space. + */ + + MatrixXd M = H * P_ext * H.transpose() + R; + + /* will use to write M as: + * + * T T + * M = P .L.D.L .P + * + * where: + * P is a permutation matrix + * L is lower triangular, with unit diagonal + * D is diagonal + */ + LDLT ldlt = M.ldlt(); + + /* solve for the identity matrix to realize the inverse this way */ + MatrixXd I = MatrixXd::Identity(M.rows(), M.cols()); + + /* -1 + * M + */ + MatrixXd M_inv = ldlt.solve(I); + + /* K(k+1) */ + MatrixXd K = P_ext * H.transpose() * M_inv; + + log && log("result", + xtag("k", skp1_ext->step_no()), + xtag("P(k+1|k)", matrix(P_ext)), + xtag("H", matrix(H)), + xtag("R", matrix(R)), + xtag("M", matrix(M)), + xtag("K", matrix(K))); + + return K; + } /*kalman_gain*/ + + rp + KalmanFilterEngine::correct1(rp const & skp1_ext, + KalmanFilterObservable const & h, + rp const & zkp1, + uint32_t j) + { + uint32_t n = skp1_ext->n_state(); + /* Kj :: [n x 1] */ + VectorXd Kj = kalman_gain1(skp1_ext, h, j); + /* H :: [m x n] */ + MatrixXd const & H = h.observable(); + VectorXd const & z = zkp1->z(); + + /* Hj :: [1 x n] the j'th row of H */ + auto const & Hj = H.row(j); + + + /* x(k+1|x) :: [n x 1] */ + VectorXd const & x_ext = skp1_ext->state_v(); + + /* P(k+1|k) :: [n x n] */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* innovj : difference between jth 'actual observation' + * and jth 'predicted observation' + */ + double innovj = z[j] - (Hj * x_ext); + + /* x(k+1) */ + VectorXd xkp1 = x_ext + (Kj * innovj); + + MatrixXd I = MatrixXd::Identity(n, n); + /* note: Kj [n x 1], Hj [1 x n], + * so Kj * Hj [n x n], with rank 1 + */ + MatrixXd Pkp1 = (I - (Kj * Hj)) * P_ext; + + return KalmanFilterStateExt::make(skp1_ext->step_no(), + skp1_ext->tm(), + xkp1, + Pkp1, + skp1_ext->transition(), + Kj, + j, + zkp1); + } /*correct1*/ + + rp + KalmanFilterEngine::correct(rp const & skp1_ext, + KalmanFilterObservable const & h, + rp const & zkp1) + { + uint32_t n = skp1_ext->n_state(); + /* K :: [n x m] */ + MatrixXd K = kalman_gain(skp1_ext, h); + MatrixXd const & H = h.observable(); + /* z_orig[] is original observation vector before reindexing */ + VectorXd const & z_orig = zkp1->z(); + /* reindex z_orig, keeping only elements that appear in + */ + VectorXd z = z_orig(h.keep()); + + /* 'ext' short for 'extrapolated' */ + VectorXd const & x_ext = skp1_ext->state_v(); + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* innov: difference between 'actual observations' + * and 'predicted observations' + */ + VectorXd innov = z - (H * x_ext); + + /* x(k+1) :: [n x 1] */ + VectorXd xkp1 = x_ext + K * innov; + MatrixXd I = MatrixXd::Identity(n, n); + MatrixXd Pkp1 = (I - K * H) * P_ext; + + return KalmanFilterStateExt::make(skp1_ext->step_no(), + skp1_ext->tm(), + xkp1, + Pkp1, + skp1_ext->transition(), + K, + -1 /*j: not used*/, + zkp1); + } /*correct*/ + + rp + KalmanFilterEngine::step(utc_nanos tkp1, + rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + rp const & zkp1) + { + rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + rp skp1 + = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); + + return skp1; + } /*step*/ + + rp + KalmanFilterEngine::step(KalmanFilterStep const & step_spec) + { + return step(step_spec.tkp1(), + step_spec.state(), + step_spec.model(), + step_spec.obs(), + step_spec.input()); + } /*step*/ + + rp + KalmanFilterEngine::step1(utc_nanos tkp1, + rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + rp const & zkp1, + uint32_t j) + { + rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + rp skp1 + = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); + + return skp1; + } /*step1*/ + + rp + KalmanFilterEngine::step1(KalmanFilterStep const & step_spec, + uint32_t j) + { + return step1(step_spec.tkp1(), + step_spec.state(), + step_spec.model(), + step_spec.obs(), + step_spec.input(), + j); + } /*step1*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterInput.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterInput.cpp new file mode 100644 index 00000000..a93a855c --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterInput.cpp @@ -0,0 +1,118 @@ +/* @file KalmanFilterInput.cpp */ + +#include "KalmanFilterInput.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "Eigen/src/Core/Matrix.h" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" +#include "xo/reflect/TaggedRcptr.hpp" + +namespace xo { + using xo::reflect::Reflect; + using xo::reflect::TaggedRcptr; + using xo::reflect::StructReflector; + using xo::scope; + using logutil::matrix; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXi; + using std::uint32_t; + + namespace kalman { + rp + KalmanFilterInput::make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd) + { + return new KalmanFilterInput(tkp1, presence, z, Rd); + } /*make*/ + + rp + KalmanFilterInput::make_present(utc_nanos tkp1, + VectorXd const & z) + { + VectorXb presence = VectorXb::Ones(z.cols()); + + return new KalmanFilterInput(tkp1, + presence, + z, + VectorXd(0) /*Rd - not using*/); + } /*make*/ + + VectorXi + KalmanFilterInput::make_kept_index() const + { + scope log(XO_DEBUG(false /*!debug_flag*/)); + + log && log(xtag("presence", matrix(presence_))); + + /* count truth values */ + uint32_t mstar = 0; + + for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) + ++mstar; + } + + log && log(xtag("m*", mstar)); + + VectorXi keep(mstar); + + /* 2nd pass, populate keep[] */ + + uint32_t jstar = 0; + + for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) { + keep[jstar] = j; + ++jstar; + } + } + + log && log("keep", matrix(keep)); + + return keep; + } /*make_kept_index*/ + + void + KalmanFilterInput::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, tkp1); + REFLECT_MEMBER(sr, presence); + REFLECT_MEMBER(sr, z); + REFLECT_MEMBER(sr, Rd); + } + } /*reflect_self*/ + + reflect::TaggedRcptr + KalmanFilterInput::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + + void + KalmanFilterInput::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterInput::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterInputToConsole.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterInputToConsole.cpp new file mode 100644 index 00000000..b3cd9d21 --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterInputToConsole.cpp @@ -0,0 +1,25 @@ +/* @file KalmanFilterInputToConsole.cpp */ + +#include "KalmanFilterInputToConsole.hpp" +#include "xo/indentlog/print/tag.hpp" + +namespace xo { + using xo::xtag; + + namespace kalman { + rp + KalmanFilterInputToConsole::make() { + return new KalmanFilterInputToConsole(); + } /*make*/ + + void + KalmanFilterInputToConsole::display(std::ostream & os) const + { + os << ""; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputToConsole.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterObservable.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterObservable.cpp new file mode 100644 index 00000000..ad90c1f1 --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterObservable.cpp @@ -0,0 +1,71 @@ +/* @file KalmanFilterObservable.cpp */ + +#include "KalmanFilterObservable.hpp" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::scope; + using logutil::matrix; + using xo::xtag; + + namespace kalman { + KalmanFilterObservable + KalmanFilterObservable::keep_all(MatrixXd H, + MatrixXd R) + { + VectorXi keep(H.rows()); + + for (uint32_t j=0; j"; + } /*display*/ + + std::string + KalmanFilterObservable::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterObservable.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterSpec.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterSpec.cpp new file mode 100644 index 00000000..45185551 --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterSpec.cpp @@ -0,0 +1,27 @@ +/* @file KalmanFilterSpec.cpp */ + +#include "KalmanFilterSpec.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::tostr; + using xo::xtag; + + namespace kalman { + void + KalmanFilterSpec::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterSpec::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterState.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterState.cpp new file mode 100644 index 00000000..411e6a26 --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterState.cpp @@ -0,0 +1,231 @@ +/* @file KalmanFilterState.cpp */ + +#include "KalmanFilterState.hpp" +#include "print_eigen.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "xo/reflect/TaggedPtr.hpp" +#include "xo/indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" +#include +#include + +namespace xo { + using xo::reflect::Reflect; + using xo::reflect::TaggedRcptr; + using xo::reflect::StructReflector; + using xo::time::utc_nanos; + using xo::rp; + using logutil::matrix; + using logutil::vector; + //using xo::scope; + using xo::xtag; + using xo::tostr; + //using Eigen::LDLT; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilterState ----- + + rp + KalmanFilterState::make() + { + return new KalmanFilterState(); + } /*make*/ + + rp + KalmanFilterState::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition) + { + return new KalmanFilterState(k, tk, + std::move(x), + std::move(P), + std::move(transition)); + } /*make*/ + + void + KalmanFilterState::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, k); + REFLECT_MEMBER(sr, tk); + REFLECT_MEMBER(sr, x); + REFLECT_MEMBER(sr, P); + } + } /*reflect_self*/ + + KalmanFilterState::KalmanFilterState() = default; + + KalmanFilterState::KalmanFilterState(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition) + : k_{k}, tk_{tk}, + x_{std::move(x)}, P_{std::move(P)}, + transition_{std::move(transition)} + {} + + TaggedRcptr + KalmanFilterState::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + + // ----- KalmanFilterExt ----- + + rp + KalmanFilterStateExt::make() + { + return new KalmanFilterStateExt(); + } /*make*/ + + rp + KalmanFilterStateExt::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk) + { + return new KalmanFilterStateExt(k, + tk, + std::move(x), + std::move(P), + std::move(transition), + std::move(K), + j, + std::move(zk)); + } /*make*/ + + void + KalmanFilterStateExt::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + /* TODO: use sr.adopt_ancestors() */ + + REFLECT_EXPLICIT_MEMBER(sr, "k", &KalmanFilterState::k_); + REFLECT_EXPLICIT_MEMBER(sr, "tk", &KalmanFilterState::tk_); + REFLECT_EXPLICIT_MEMBER(sr, "x", &KalmanFilterState::x_); + REFLECT_EXPLICIT_MEMBER(sr, "P", &KalmanFilterState::P_); + REFLECT_EXPLICIT_MEMBER(sr, "transition", &KalmanFilterState::transition_); + REFLECT_MEMBER(sr, j); + REFLECT_MEMBER(sr, K); + REFLECT_MEMBER(sr, zk); + } + } /*reflect_self*/ + + KalmanFilterStateExt::KalmanFilterStateExt(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk) + : KalmanFilterState(k, tk, + std::move(x), + std::move(P), + std::move(transition)), + j_{j}, + K_{std::move(K)}, + zk_{std::move(zk)} + { + uint32_t n = x.size(); + + if (n != P.rows() || n != P.cols()) { + std::string err_msg + = tostr("with n=x.size expect [n x n] covar matrix P", + xtag("n", x.size()), + xtag("P.rows", P.rows()), + xtag("P.cols", P.cols())); + + throw std::runtime_error(err_msg); + } + + if ((K.rows() > 0) && (K.rows() > 0)) { + if (n != K.rows()) { + std::string err_msg + = tostr("with n=x.size expect [m x n] gain matrix K", + xtag("n", x.size()), + xtag("K.rows", K.rows()), + xtag("K.cols", K.cols())); + + throw std::runtime_error(err_msg); + } + } else { + /* bypass test with [0 x 0] matrix K; + * normal for initial filter state + */ + } + } /*ctor*/ + + void + KalmanFilterState::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterState::display_string() const + { + std::stringstream ss; + ss << *this; + return ss.str(); + } /*display_string*/ + + // ----- KalmanFilterStateExt ----- + + rp + KalmanFilterStateExt::initial(utc_nanos t0, + VectorXd x0, + MatrixXd P0) + { + return KalmanFilterStateExt::make + (0 /*k*/, + t0, + std::move(x0), + std::move(P0), + KalmanFilterTransition(MatrixXd() /*F - not used for initial step*/, + MatrixXd() /*Q - not used for initial step*/), + MatrixXd() /*K - not used for initial step*/, + -1 /*j - not used for initial step*/, + nullptr /*zk - not defined for initial step*/); + } /*initial*/ + + void + KalmanFilterStateExt::display(std::ostream & os) const + { + os << "step_no()) + << xtag("tk", this->tm()) + << xtag("x", matrix(this->state_v())) + << xtag("P", matrix(this->state_cov())) + << xtag("K", matrix(K_)) + << xtag("j", j_) + << ">"; + } /*display*/ + + TaggedRcptr + KalmanFilterStateExt::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterStateToConsole.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterStateToConsole.cpp new file mode 100644 index 00000000..c7043e2c --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterStateToConsole.cpp @@ -0,0 +1,25 @@ +/* @file KalmanFilterStateToConsole.cpp */ + +#include "KalmanFilterStateToConsole.hpp" +#include "xo/indentlog/print/tag.hpp" + +namespace xo { + using xo::xtag; + + namespace kalman { + rp + KalmanFilterStateToConsole::make() { + return new KalmanFilterStateToConsole(); + } /*make*/ + + void + KalmanFilterStateToConsole::display(std::ostream & os) const + { + os << ""; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStateToConsole.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterStep.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterStep.cpp new file mode 100644 index 00000000..7313924f --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterStep.cpp @@ -0,0 +1,84 @@ +/* @file KalmanFilterStep.cpp */ + +#include "KalmanFilterStep.hpp" +#include "KalmanFilterEngine.hpp" +#include "KalmanFilterState.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::scope; + using xo::tostr; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + rp + KalmanFilterStep::extrapolate() const + { + return KalmanFilterEngine::extrapolate(this->tkp1(), + this->state(), + this->model() /*transition*/); + } /*extrapolate*/ + + MatrixXd + KalmanFilterStep::gain(rp const & skp1_ext) const + { + return KalmanFilterEngine::kalman_gain(skp1_ext, + this->obs()); + } /*gain*/ + + VectorXd + KalmanFilterStep::gain1(rp const & skp1_ext, + uint32_t j) const + { + return KalmanFilterEngine::kalman_gain1(skp1_ext, + this->obs(), + j); + + } /*gain1*/ + + rp + KalmanFilterStep::correct(rp const & skp1_ext) + { + return KalmanFilterEngine::correct(skp1_ext, + this->obs(), + this->input()); + } /*correct*/ + + rp + KalmanFilterStep::correct1(rp const & skp1_ext, + uint32_t j) + { + return KalmanFilterEngine::correct1(skp1_ext, + this->obs(), + this->input(), + j); + } /*correct1*/ + + void + KalmanFilterStep::display(std::ostream & os) const + { + //scope lscope("KalmanFilterStep::display"); + + os << "model()); + //lscope.log("obs:"); + os << xtag("obs", this->obs()); + //lscope.log("input:"); + os << xtag("input", this->input()); + os << ">"; + } /*display*/ + + std::string + KalmanFilterStep::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStep.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterSvc.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterSvc.cpp new file mode 100644 index 00000000..c1392dfd --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterSvc.cpp @@ -0,0 +1,44 @@ +/* @file KalmanFilterSvc.cpp */ + +#include "KalmanFilterSvc.hpp" + +namespace xo { + using xo::rp; + using xo::scope; + using xo::xtag; + + namespace kalman { + rp + KalmanFilterSvc::make(KalmanFilterSpec spec) + { + return new KalmanFilterSvc(std::move(spec)); + } /*make*/ + + KalmanFilterSvc::KalmanFilterSvc(KalmanFilterSpec spec) + : filter_{std::move(spec)} + {} + + void + KalmanFilterSvc::notify_ev(rp const & input_kp1) + { + this->filter_.notify_input(input_kp1); + + ++(this->n_in_ev_); + this->notify_secondary_event(this->filter_.state_ext()); + } /*notify_input*/ + + void + KalmanFilterSvc::display(std::ostream & os) const + { + os << "name()) + << xtag("n_in_ev", this->n_in_ev()) + << xtag("n_queued_out_ev", this->n_queued_out_ev()) + << xtag("n_out_ev", this->n_out_ev()) + //<< xtag("filter", this->filter_) + << ">"; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSvc.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/KalmanFilterTransition.cpp b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterTransition.cpp new file mode 100644 index 00000000..a6ec63ae --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/KalmanFilterTransition.cpp @@ -0,0 +1,55 @@ +/* @file KalmanFilterTransition.cpp */ + +#include "KalmanFilterTransition.hpp" +#include "print_eigen.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::reflect::StructReflector; + using logutil::matrix; + using xo::xtag; + + namespace kalman { + void + KalmanFilterTransition::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, F); + REFLECT_MEMBER(sr, Q); + } + } /*reflect_self*/ + + uint32_t + KalmanFilterTransition::n_state() const + { + /* we know F.rows() == F.cols() = Q.cols() == Q.rows(), + * see .check_ok() + */ + + return F_.rows(); + } /*n_state*/ + + void + KalmanFilterTransition::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterTransition::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterTransition.cpp */ diff --git a/xo-kalmanfilter/src/kalmanfilter/init_filter.cpp b/xo-kalmanfilter/src/kalmanfilter/init_filter.cpp new file mode 100644 index 00000000..3417214b --- /dev/null +++ b/xo-kalmanfilter/src/kalmanfilter/init_filter.cpp @@ -0,0 +1,52 @@ +/* file init_filter.cpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#include "init_filter.hpp" +#include "xo/reactor/init_reactor.hpp" + +#include "KalmanFilterState.hpp" +#include "EigenUtil.hpp" +#include "xo/printjson/PrintJson.hpp" + +namespace xo { + using xo::kalman::KalmanFilterInput; + using xo::kalman::KalmanFilterTransition; + using xo::kalman::KalmanFilterState; + using xo::kalman::KalmanFilterStateExt; + using xo::eigen::EigenUtil; + using xo::json::PrintJsonSingleton; + using xo::json::PrintJson; + + void + InitSubsys::init() + { + PrintJson * pjson = PrintJsonSingleton::instance().get(); + + EigenUtil::reflect_eigen(); + EigenUtil::provide_json_printers(pjson); + + KalmanFilterInput::reflect_self(); + KalmanFilterTransition::reflect_self(); + KalmanFilterState::reflect_self(); + KalmanFilterStateExt::reflect_self(); + + } /*init*/ + + InitEvidence + InitSubsys::require() + { + InitEvidence retval; + + /* subsystem dependencies for filter/ */ + retval ^= InitSubsys::require(); + + /* filter/'s own initialization code */ + retval ^= Subsystem::provide("kalmanfilter", &init); + + return retval; + } /*require*/ +} /*namespace xo*/ + +/* end init_filter.cpp */ diff --git a/xo-kalmanfilter/utest/CMakeLists.txt b/xo-kalmanfilter/utest/CMakeLists.txt new file mode 100644 index 00000000..1cd562ad --- /dev/null +++ b/xo-kalmanfilter/utest/CMakeLists.txt @@ -0,0 +1,23 @@ +# xo-kalmanfilter/utest/CMakeLists.txt + +set(SELF_EXE utest.filter) + +set(SELF_SRCS + KalmanFilter.test.cpp + filter_utest_main.cpp) + +xo_add_utest_executable(${SELF_EXE} ${SELF_SRCS}) + +# ---------------------------------------------------------------- +# create convenience symlink from build dir back to canned data. +# This is ok since we don't implement install for unit tests + +#create_symlink("${CMAKE_SOURCE_DIR}/utestdata/" "src/filter/utest/utestdata") +file(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/utest/utestdata) +file(CREATE_LINK ${PROJECT_SOURCE_DIR}/utest/utestdata/filter ${PROJECT_BINARY_DIR}/utest/utestdata/filter SYMBOLIC) + +xo_self_dependency(${SELF_EXE} xo_kalmanfilter) +xo_dependency(${SELF_EXE} xo_statistics) +xo_external_target_dependency(${SELF_EXE} Catch2 Catch2::Catch2) + +# end CMakeLists.txt diff --git a/xo-kalmanfilter/utest/KalmanFilter.test.cpp b/xo-kalmanfilter/utest/KalmanFilter.test.cpp new file mode 100644 index 00000000..a310adb4 --- /dev/null +++ b/xo-kalmanfilter/utest/KalmanFilter.test.cpp @@ -0,0 +1,687 @@ +/* @file KalmanFilter.test.cpp */ + +#include "xo/kalmanfilter/KalmanFilter.hpp" +#include "xo/kalmanfilter/KalmanFilterEngine.hpp" +#include "xo/kalmanfilter/print_eigen.hpp" +#include "xo/statistics/SampleStatistics.hpp" +#include "xo/randomgen/normalgen.hpp" +#include "xo/randomgen/xoshiro256.hpp" +#include "xo/indentlog/scope.hpp" +#include "xo/indentlog/log_level.hpp" +#include +#include + +namespace xo { + using xo::kalman::KalmanFilterSpec; + using xo::kalman::KalmanFilterStep; + using xo::kalman::KalmanFilterEngine; + using xo::kalman::KalmanFilterStateExt; + using xo::kalman::KalmanFilterState; + using xo::kalman::KalmanFilterTransition; + using xo::kalman::KalmanFilterObservable; + using xo::kalman::KalmanFilterInput; + using xo::statistics::SampleStatistics; + using xo::rng::normalgen; + using xo::rng::xoshiro256ss; + using xo::time::timeutil; + using xo::time::utc_nanos; + using xo::time::seconds; + using xo::rp; + using xo::log_level; + using logutil::matrix; + using xo::print::ccs; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace ut { + namespace { + /* step for kalman filter with: + * - single state variable x[0] + * - identity process model - x(k+1) = F(k).x(k), with F(k) = | 1 | + * - no process noise + * - single observation z[0] + * - identity coupling matrix - z(k) = H(k).x(k) + w(k), with H(k) = | 1 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity1_mkstep_fn() + { + /* kalman state transition matrix: use identity <--> state is constant */ + MatrixXd F = MatrixXd::Identity(1, 1); + + /* state transition noise: set this to zero; + * measuring something that's known to be constant + */ + MatrixXd Q = MatrixXd::Zero(1, 1); + + /* single direct observation */ + MatrixXd H = MatrixXd::Identity(1, 1); + + /* observation errors understood to have + * mean 0, sdev 1 + * + * This is consistent with normal_rng below, + * so R is correctly specified + */ + MatrixXd R = MatrixXd::Identity(1, 1); + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity1_mkstep_fn*/ + } /*namespace*/ + + /* example 1. + * repeated direct observation of a scalar + * use rng to generate observations + */ + TEST_CASE("kalman-identity", "[kalmanfilter]") { + /* setting up trivial filter for repeated indept + * measurements of a constant. + * + * True value of unknown set to 10, + * utest observes filter converging toward that value + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + /* F, Q, K, j, zk not used for initial state */ + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't matter */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 10.0 + normal_rng(); + + INFO(tostr("z=", z)); + + z_stats.include_sample(z[0]); + + rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + + rp skp1 + = KalmanFilterEngine::step(step_spec); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->observable() == -1); + + /* z_stats reflects k = z_stats.n_sample() N(0,1) 'random' vars; + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(0.01).epsilon(1e-6)); + REQUIRE(sk->gain()(0, 0) == Approx(0.01).epsilon(1e-6)); + } /*TEST_CASE(kalman-identity)*/ + + /* example 2. + * like example 1, but using "separate observation" variants: + * KalmanGain::correct1() // per observation + * instead of + * KalmanGain::correct() // per observation set + */ + TEST_CASE("kalman-identity1", "[kalmanfilter]") { + /* setting up trivial filter for repeated indept + * measurements of a constant. + * + * True value of unknown set to 10, + * utest observes filter converging toward that value + * + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't matter */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 10.0 + normal_rng(); + + INFO(tostr("z=", z)); + + z_stats.include_sample(z[0]); + + rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec + = spec.make_step(sk, inputk); + + rp skp1 + = KalmanFilterEngine::step1(step_spec, 0 /*j*/); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->observable() == 0); + + /* z_stats reflects k = z_stats.n_sample() N(0,1) 'random' vars; + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(0.01).epsilon(1e-6)); + REQUIRE(sk->gain()(0, 0) == Approx(0.01).epsilon(1e-6)); + } /*TEST_CASE(kalman-identity1)*/ + + namespace { + /* step for kalman filter with: + * - single state variable x[0] + * - identity process model: x(k+1) = F(k).x(k), with F(k) = | 1 | + * - no process noise + * - two observations z[0], z[1] + * - identity coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 | + * | 1 | + * + * w(k) = | w1 | with w1 ~ N(0,1) + * | w2 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity2_mkstep_fn() + { + /* kalman state transition matrix: use identity <-> state is constant */ + MatrixXd F = MatrixXd::Identity(1, 1); + + /* state transition noise: set to 0 */ + MatrixXd Q = MatrixXd::Zero(1, 1); + + /* two direct observations */ + MatrixXd H = MatrixXd::Constant(2 /*#rows*/, 1 /*#cols*/, 1.0 /*M(i,j)*/); + + /* observation errors: N(0,1) */ + MatrixXd R = MatrixXd::Identity(2, 2); + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity2_mkstep_fn*/ + } /*namespace*/ + + TEST_CASE("kalman-identity2", "[kalmanfilter]") { + /* variation on filter in kalman-identity1 utest above; + * this time make 2 observations per step + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity2_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + rp sk = spec.start_ext(); + + /* need 1/2 as many filter steps to reach same confidence + * as in test "kalman-identity" + */ + for(uint32_t i_step = 1; i_step < 51; ++i_step) { + INFO(tostr(xtag("i_step", i_step))); + + /* note: for this filter, measurement time doesn't affect behavior */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(2); + z << 10.0 + normal_rng(), 10.0 + normal_rng(); + + z_stats.include_sample(z[0]); + z_stats.include_sample(z[1]); + + INFO(tostr(xtag("i_step", i_step), xtag("z", z))); + + rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + + rp skp1 = KalmanFilterEngine::step(step_spec); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 2); + REQUIRE(skp1->observable() == -1); + /* z_stats reflects 2*k = z_stats.n_sample() N(0,1) 'random' vars + * (since 2 vars per step) + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + * + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + REQUIRE(skp1->gain()(0, 1) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + /* result is close but not identical, + * because initial confidence P0 counts as one sample, + * so have odd #of samples + */ + REQUIRE(sk->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + REQUIRE(sk->gain()(0, 1) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + } /*TEST_CASE(kalman-identity2)*/ + + namespace { + /* step for kalman filter with: + * - two state variables x[0], x[1] + * x[0] subject to random disturbances, reverts towards mean 1 + * x[1] is 1 + * - process model: x(k+1) = F(k).x(k) + v(k) with + * F(k) = | 0.95 0.05 | v(k) = | v1 |, v ~ N(0, 0.25) + * | 0 1 | | 0 | + * - one observation z[0] + * - coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 0 | + * + * w(k) ~ N(0,1) + */ + KalmanFilterSpec::MkStepFn + kalman_revert1_mkstep_fn() + { + /* kalman state transition matrix */ + MatrixXd F(2,2); + F << 0.95, 0.05, 0, 1; + + /* state transition noise */ + MatrixXd Q(2,2); + Q << 0.0001, 0.0, 0.0, 0.0; + + /* coupling matrix */ + MatrixXd H(1,2); + H << 1.0, 0.0; + + /* observation errors */ + MatrixXd R(1,1); + R << 0.25; + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_revert1_mkstep_fn*/ + } /*namespace*/ + + TEST_CASE("kalman-revert1", "[kalmanfilter]") { + /* like test "kalman-identity", + * but introduce some system noise. + */ + + constexpr bool c_debug_enabled = false; + scope lscope(XO_DEBUG2(c_debug_enabled, "TEST(kalman_revert)")); + + /* seed for rng */ + uint64_t seed = 14950139742636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on observations, + * use as reference when assessing filter behavior + */ + SampleStatistics z_stats; + + /* write output to file - use as baseline for regression testing */ + std::string self_test_name = Catch::getResultCapture().getCurrentTestName(); + + /* write space-delimited output, suitable for gnuplot + * omit always-constant values, rely on unit test verifying these + */ + std::ofstream out(self_test_name); + out << "step z0 x0 P00 K0" << std::endl; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0). + * x(0)[1] is constant 1, used to achieve mean reversion + */ + VectorXd x0(2); + x0 << 1.0 + normal_rng(), 1.0; + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0(2,2); + P0 << 1.0, 0.0, 0.0, 0.0; + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_revert1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't affect behavior */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 1.0 + normal_rng(); + + z_stats.include_sample(z[0]); + + rp inputk + = KalmanFilterInput::make_present(tkp1, z); + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + rp skp1 = KalmanFilterEngine::step(step_spec); + + if (c_debug_enabled) { + lscope.log("filter", + xtag("step", i_step), + xtag("z", matrix(z)), + xtag("x", matrix(skp1->state_v())), + xtag("P", matrix(skp1->state_cov())), + xtag("K", matrix(skp1->gain()))); + } + + /* headings: step z0 x0 P00 K0 */ + out << i_step + << " " << z(0) + << " " << skp1->state_v()(0) + << " " << skp1->state_cov()(0, 0) + << " " << skp1->gain()(0, 0) + << "\n"; + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 2); + // + REQUIRE(skp1->state_v().size() == 2); + REQUIRE(skp1->state_v()(1) == 1.0); + // + REQUIRE(skp1->state_cov().rows() == 2); + REQUIRE(skp1->state_cov().cols() == 2); + // test skp1->state_cov()(0,0) vs baseline + REQUIRE(skp1->state_cov()(0, 0) >= 0.0); + REQUIRE(skp1->state_cov()(1, 0) == 0.0); + REQUIRE(skp1->state_cov()(0, 1) == 0.0); + REQUIRE(skp1->state_cov()(1, 1) == 0.0); + // + REQUIRE(skp1->gain().rows() == 2); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->gain()(0, 0) > 0.0); + REQUIRE(skp1->gain()(1, 0) == 0.0); + // + REQUIRE(skp1->observable() == -1); + // + + sk = skp1; + } + + out << std::flush; + out.close(); + + /* compare output with regression baseline. + * command like: + * diff kalman-revert1 utestdata/filter/kalman-revert1 + */ + char cmd_buf[256]; + snprintf(cmd_buf, sizeof(cmd_buf), + "diff %s utestdata/filter/%s\n", + self_test_name.c_str(), + self_test_name.c_str()); + + INFO(tostr(self_test_name, xtag("cmd", ccs(cmd_buf)))); + + std::int32_t err = ::system(cmd_buf); + + REQUIRE(err == 0); + } /*TEST_CASE(kalman-drift)*/ + +#ifdef NOT_IN_USE + namespace { + /* step for kalman filter with: + * - two state variables x[0], x[1] + * - identity process model: x(k+1) = F(k).x(k), with + * F(k) = | 1 0 | + * | 0 1 | + * - no process noise + * - two observations z[0], z[1] + * - simple coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 0 | + * | 0 -1 | + * (so sign of z[1] is reversed w.r.t x[1]) + * + * w(k) = | w1 | with w1 ~ N(0,1) + * | w2 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity2x2_mkstep_fn() + { + /* kalman state transition matrix: use identity <-> state is constant */ + MatrixXd F = MatrixXd::Identity(2, 2); + + /* state transition noise: set to 0 */ + MatrixXd Q = MatrixXd::Zero(2, 2); + + /* two direct observations */ + MatrixXd H = MatrixXd::Constant(2 /*#rows*/, 1 /*#cols*/, 1.0 /*M(i,j)*/); + + /* observation errors: N(0,1) */ + MatrixXd R = MatrixXd::Identity(2, 2); + + return [F, Q, H, R](KalmanFilterState const & sk, + KalmanFilterInput const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity2_mkstep_fn*/ + } /*namespace*/ +#endif + } /*namespace ut*/ +} /*namespace xo*/ + +/* end KalmanFilter.test.cpp */ diff --git a/xo-kalmanfilter/utest/filter_utest_main.cpp b/xo-kalmanfilter/utest/filter_utest_main.cpp new file mode 100644 index 00000000..9ad0266a --- /dev/null +++ b/xo-kalmanfilter/utest/filter_utest_main.cpp @@ -0,0 +1,6 @@ +/* @file filter_utest_main.cpp */ + +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" + +/* end filter_utest_main.cpp */ diff --git a/xo-kalmanfilter/utest/utestdata/filter/kalman-revert1 b/xo-kalmanfilter/utest/utestdata/filter/kalman-revert1 new file mode 100644 index 00000000..343af232 --- /dev/null +++ b/xo-kalmanfilter/utest/utestdata/filter/kalman-revert1 @@ -0,0 +1,100 @@ +step z0 x0 P00 K0 +1 1.33055 1.28103 0.195775 0.783099 +2 1.26559 1.2664 0.103557 0.414227 +3 0.790814 1.1272 0.0680813 0.272325 +4 0.173362 0.933668 0.0493859 0.197543 +5 2.12197 1.11662 0.0378989 0.151595 +6 1.00197 1.09766 0.0301647 0.120659 +7 -0.312247 0.954347 0.0246315 0.0985261 +8 1.76439 1.02286 0.020499 0.081996 +9 0.518422 0.986867 0.0173123 0.0692491 +10 0.756683 0.973864 0.0147938 0.0591754 +11 -0.609708 0.894249 0.0127646 0.0510585 +12 0.330435 0.874259 0.011104 0.0444159 +13 1.03713 0.886639 0.00972751 0.03891 +14 -0.994029 0.827609 0.00857454 0.0342982 +15 1.58353 0.858947 0.00760022 0.0304009 +16 0.494725 0.855945 0.00677073 0.0270829 +17 1.0205 0.866962 0.00606004 0.0242401 +18 1.98651 0.897865 0.00544782 0.0217913 +19 1.70559 0.918761 0.00491797 0.0196719 +20 1.44881 0.932201 0.00445755 0.0178302 +21 2.16318 0.955508 0.00405605 0.0162242 +22 0.353787 0.948782 0.00370485 0.0148194 +23 1.72676 0.961879 0.00339684 0.0135874 +24 -1.55312 0.932313 0.00312606 0.0125043 +25 -0.523229 0.918847 0.00288753 0.0115501 +26 1.81676 0.932476 0.00267702 0.0107081 +27 1.65387 0.943006 0.00249094 0.00996377 +28 -0.766534 0.929922 0.00232623 0.00930491 +29 1.1143 0.935004 0.00218024 0.00872095 +30 0.908743 0.938011 0.0020507 0.00820282 +31 1.80832 0.947825 0.00193566 0.00774263 +32 1.61306 0.955293 0.00183339 0.00733354 +33 1.10591 0.958563 0.0017424 0.00696961 +34 1.54404 0.964512 0.0016614 0.00664561 +35 1.47595 0.969526 0.00158925 0.00635699 +36 -1.16643 0.958012 0.00152494 0.00609975 +37 2.772 0.970748 0.00146759 0.00587036 +38 0.0147533 0.966786 0.00141643 0.00566572 +39 1.5648 0.971716 0.00137077 0.00548308 +40 0.758209 0.971987 0.00133001 0.00532003 +41 0.602221 0.971467 0.0012936 0.00517441 +42 1.02795 0.973171 0.00126108 0.00504433 +43 1.61126 0.977651 0.00123203 0.0049281 +44 3.23237 0.98964 0.00120606 0.00482423 +45 -1.20629 0.979766 0.00118284 0.00473137 +46 2.34054 0.987098 0.00116209 0.00464835 +47 1.886 0.991852 0.00114353 0.00457412 +48 1.62241 0.9951 0.00112693 0.00450773 +49 0.10733 0.991395 0.00111209 0.00444835 +50 0.111089 0.987954 0.00109881 0.00439523 +51 -0.045289 0.984062 0.00108693 0.00434771 +52 0.339478 0.98208 0.0010763 0.0043052 +53 -0.109018 0.978316 0.00106679 0.00426715 +54 -1.15684 0.970358 0.00105828 0.00423311 +55 -0.289587 0.966538 0.00105066 0.00420265 +56 1.04772 0.968543 0.00104385 0.00417538 +57 -0.0931435 0.965703 0.00103774 0.00415098 +58 2.56415 0.974011 0.00103228 0.00412914 +59 0.352559 0.972751 0.0010274 0.00410959 +60 0.173725 0.970838 0.00102302 0.00409209 +61 0.80765 0.971625 0.00101911 0.00407643 +62 2.41789 0.978913 0.0010156 0.0040624 +63 2.63322 0.986663 0.00101246 0.00404985 +64 2.59753 0.993833 0.00100965 0.00403861 +65 0.530642 0.992274 0.00100714 0.00402855 +66 1.24782 0.993686 0.00100489 0.00401955 +67 0.695031 0.992802 0.00100287 0.00401149 +68 2.80672 1.00042 0.00100107 0.00400427 +69 0.122651 0.996894 0.000999451 0.0039978 +70 1.22154 0.997945 0.000998005 0.00399202 +71 0.269797 0.995145 0.00099671 0.00398684 +72 1.78662 0.998538 0.00099555 0.0039822 +73 0.4817 0.996555 0.000994512 0.00397805 +74 1.05221 0.996948 0.000993582 0.00397433 +75 1.75822 1.00012 0.00099275 0.003971 +76 1.31536 1.00137 0.000992005 0.00396802 +77 1.29006 1.00244 0.000991338 0.00396535 +78 0.857458 1.00175 0.000990741 0.00396296 +79 0.269498 0.998761 0.000990206 0.00396082 +80 0.131184 0.995388 0.000989727 0.00395891 +81 0.736283 0.994592 0.000989298 0.00395719 +82 -0.32478 0.989642 0.000988914 0.00395566 +83 1.3352 0.991525 0.00098857 0.00395428 +84 0.279734 0.989133 0.000988263 0.00395305 +85 1.74807 0.992673 0.000987987 0.00395195 +86 1.82522 0.996328 0.00098774 0.00395096 +87 -0.281213 0.991464 0.000987519 0.00395008 +88 1.61161 0.994338 0.000987322 0.00394929 +89 2.63571 1.0011 0.000987144 0.00394858 +90 -0.139888 0.996542 0.000986986 0.00394794 +91 3.48906 1.00655 0.000986844 0.00394738 +92 2.01113 1.01019 0.000986717 0.00394687 +93 1.84914 1.01299 0.000986603 0.00394641 +94 0.480262 1.01025 0.000986501 0.003946 +95 1.45288 1.01148 0.00098641 0.00394564 +96 -0.0838873 1.00659 0.000986328 0.00394531 +97 -0.570744 1.00004 0.000986255 0.00394502 +98 -0.214659 0.995244 0.000986189 0.00394476 +99 1.7904 0.998618 0.000986131 0.00394452