Add 'xo-kalmanfilter/' from commit '2ced8429c0'
git-subtree-dir: xo-kalmanfilter git-subtree-mainline:a49aa0c1f1git-subtree-split:2ced8429c0
This commit is contained in:
commit
9e93a96754
42 changed files with 4220 additions and 0 deletions
488
xo-kalmanfilter/.github/workflows/main.yml
vendored
Normal file
488
xo-kalmanfilter/.github/workflows/main.yml
vendored
Normal file
|
|
@ -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}})
|
||||
6
xo-kalmanfilter/.gitignore
vendored
Normal file
6
xo-kalmanfilter/.gitignore
vendored
Normal file
|
|
@ -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
|
||||
30
xo-kalmanfilter/CMakeLists.txt
Normal file
30
xo-kalmanfilter/CMakeLists.txt
Normal file
|
|
@ -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
|
||||
164
xo-kalmanfilter/EXAMPLES
Normal file
164
xo-kalmanfilter/EXAMPLES
Normal file
|
|
@ -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)
|
||||
|
||||
56
xo-kalmanfilter/README.md
Normal file
56
xo-kalmanfilter/README.md
Normal file
|
|
@ -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
|
||||
```
|
||||
35
xo-kalmanfilter/cmake/xo-bootstrap-macros.cmake
Normal file
35
xo-kalmanfilter/cmake/xo-bootstrap-macros.cmake
Normal file
|
|
@ -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()
|
||||
17
xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in
Normal file
17
xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in
Normal file
|
|
@ -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@")
|
||||
29
xo-kalmanfilter/include/xo/kalmanfilter/EigenUtil.hpp
Normal file
29
xo-kalmanfilter/include/xo/kalmanfilter/EigenUtil.hpp
Normal file
|
|
@ -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<bool, Eigen::Dynamic, 1>; 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 */
|
||||
127
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilter.hpp
Normal file
127
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilter.hpp
Normal file
|
|
@ -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<KalmanFilterStateExt> 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<KalmanFilterInput> 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<KalmanFilterStateExt> 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 */
|
||||
185
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterEngine.hpp
Normal file
185
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterEngine.hpp
Normal file
|
|
@ -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<KalmanFilterState> extrapolate(utc_nanos tkp1,
|
||||
rp<KalmanFilterState> 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<KalmanFilterState> 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<KalmanFilterState> 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<KalmanFilterStateExt> correct(rp<KalmanFilterState> const & skp1_ext,
|
||||
KalmanFilterObservable const & Hkp1,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt> correct1(rp<KalmanFilterState> const & skp1_ext,
|
||||
KalmanFilterObservable const & Hkp1,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt> step(utc_nanos tkp1,
|
||||
rp<KalmanFilterState> const & sk,
|
||||
KalmanFilterTransition const & Fk,
|
||||
KalmanFilterObservable const & Hkp1,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt> 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<KalmanFilterStateExt> step1(utc_nanos tkp1,
|
||||
rp<KalmanFilterState> const & sk,
|
||||
KalmanFilterTransition const & Fk,
|
||||
KalmanFilterObservable const & Hkp1,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt> step1(KalmanFilterStep const & step_spec,
|
||||
uint32_t j);
|
||||
}; /*KalmanFilterEngine*/
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterEngine.hpp */
|
||||
121
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInput.hpp
Normal file
121
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterInput.hpp
Normal file
|
|
@ -0,0 +1,121 @@
|
|||
/* @file KalmanFilterInput.hpp */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "xo/reflect/SelfTagging.hpp"
|
||||
//#include "time/Time.hpp"
|
||||
//#include "xo/refcnt/Refcounted.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include <cstdint>
|
||||
|
||||
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<bool, Eigen::Dynamic, 1>;
|
||||
using VectorXi = Eigen::VectorXi;
|
||||
using VectorXd = Eigen::VectorXd;
|
||||
using uint32_t = std::uint32_t;
|
||||
|
||||
public:
|
||||
KalmanFilterInput() = default;
|
||||
|
||||
static rp<KalmanFilterInput> 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<KalmanFilterInput> 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<KalmanFilterInput>;
|
||||
|
||||
inline std::ostream &
|
||||
operator<<(std::ostream & os, KalmanFilterInput const & x)
|
||||
{
|
||||
x.display(os);
|
||||
return os;
|
||||
} /*operator<<*/
|
||||
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterInput.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<rp<KalmanFilterInput>>;
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterInputCallback.hpp */
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
/* @file KalmanFilterInputSource.hpp */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "xo/reactor/EventSource.hpp"
|
||||
#include "KalmanFilterInputCallback.hpp"
|
||||
|
||||
namespace xo {
|
||||
namespace kalman {
|
||||
/* Use:
|
||||
* rp<KalmanFilterInputSource> src = ...;
|
||||
* rp<KalmanFilterInputCallback> 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 */
|
||||
|
|
@ -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<rp<KalmanFilterInput>>
|
||||
{
|
||||
public:
|
||||
KalmanFilterInputToConsole() = default;
|
||||
|
||||
static rp<KalmanFilterInputToConsole> 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 */
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
/* @file KalmanFilterObservable.hpp */
|
||||
|
||||
#pragma once
|
||||
|
||||
//#include "time/Time.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include <cstdint>
|
||||
|
||||
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 */
|
||||
|
|
@ -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<rp<KalmanFilterStateExt>>;
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterOutputCallback.hpp */
|
||||
86
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSpec.hpp
Normal file
86
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSpec.hpp
Normal file
|
|
@ -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<KalmanFilterStep
|
||||
(rp<KalmanFilterState> const & sk,
|
||||
KalmanFilterInputPtr const & zkp1)>;
|
||||
|
||||
public:
|
||||
explicit KalmanFilterSpec(rp<KalmanFilterStateExt> s0, MkStepFn mkstepfn)
|
||||
: start_ext_{std::move(s0)},
|
||||
mk_step_fn_{std::move(mkstepfn)} {}
|
||||
|
||||
rp<KalmanFilterStateExt> 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<KalmanFilterState> const & sk,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt> 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 */
|
||||
163
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterState.hpp
Normal file
163
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterState.hpp
Normal file
|
|
@ -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 <Eigen/Dense>
|
||||
#include <functional>
|
||||
#include <cstdint>
|
||||
|
||||
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<KalmanFilterState> make();
|
||||
static rp<KalmanFilterState> 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<KalmanFilterStateExt> make();
|
||||
static rp<KalmanFilterStateExt> make(uint32_t k,
|
||||
utc_nanos tk,
|
||||
VectorXd x,
|
||||
MatrixXd P,
|
||||
KalmanFilterTransition transition,
|
||||
MatrixXd K,
|
||||
int32_t j,
|
||||
rp<KalmanFilterInput> zk);
|
||||
|
||||
/* create state object for initial filter state */
|
||||
static rp<KalmanFilterStateExt> 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<KalmanFilterInput> 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<KalmanFilterInput> 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<KalmanFilterInput> zk_;
|
||||
}; /*KalamnFilterStateExt*/
|
||||
} /*namespace filter*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterState.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<KalmanFilterStateExt>
|
||||
{
|
||||
public:
|
||||
KalmanFilterStateToConsole() = default;
|
||||
|
||||
static rp<KalmanFilterStateToConsole> 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 */
|
||||
128
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStep.hpp
Normal file
128
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterStep.hpp
Normal file
|
|
@ -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<KalmanFilterState> state,
|
||||
KalmanFilterTransition model,
|
||||
KalmanFilterObservable obs,
|
||||
rp<KalmanFilterInput> zkp1)
|
||||
: KalmanFilterStepBase(model, obs),
|
||||
state_{std::move(state)},
|
||||
input_{std::move(zkp1)} {}
|
||||
|
||||
rp<KalmanFilterState> const & state() const { return state_; }
|
||||
rp<KalmanFilterInput> 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<KalmanFilterState> 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<KalmanFilterState> 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<KalmanFilterState> 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<KalmanFilterStateExt> correct(rp<KalmanFilterState> 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<KalmanFilterStateExt> correct1(rp<KalmanFilterState> 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<KalmanFilterState> 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 */
|
||||
64
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSvc.hpp
Normal file
64
xo-kalmanfilter/include/xo/kalmanfilter/KalmanFilterSvc.hpp
Normal file
|
|
@ -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<rp<KalmanFilterInput>>,
|
||||
public xo::reactor::DirectSourcePtr<rp<KalmanFilterStateExt>> {
|
||||
public:
|
||||
using AbstractSource = xo::reactor::AbstractSource;
|
||||
|
||||
public:
|
||||
/* named ctor idiom */
|
||||
static rp<KalmanFilterSvc> make(KalmanFilterSpec spec);
|
||||
|
||||
KalmanFilter const & filter() const { return filter_; }
|
||||
|
||||
/* notify incoming observations; will trigger kalman filter step */
|
||||
void notify_ev(rp<KalmanFilterInput> 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<KalmanFilterInputSource> 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<KalmanFilterOutputCallback> pub_;
|
||||
}; /*KalmanFilterSvc*/
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* KalmanFilterSvc.hpp */
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/* @file KalmanFilterTransition.hpp */
|
||||
|
||||
#pragma once
|
||||
|
||||
//#include "time/Time.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include <cstdint>
|
||||
|
||||
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 */
|
||||
20
xo-kalmanfilter/include/xo/kalmanfilter/init_filter.hpp
Normal file
20
xo-kalmanfilter/include/xo/kalmanfilter/init_filter.hpp
Normal file
|
|
@ -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<S_kalmanfilter_tag> {
|
||||
static void init();
|
||||
static InitEvidence require();
|
||||
};
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end init_kalmanfilter.hpp */
|
||||
41
xo-kalmanfilter/include/xo/kalmanfilter/print_eigen.hpp
Normal file
41
xo-kalmanfilter/include/xo/kalmanfilter/print_eigen.hpp
Normal file
|
|
@ -0,0 +1,41 @@
|
|||
/* @file print_eigen.hpp */
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <cstdint>
|
||||
|
||||
namespace logutil {
|
||||
template<typename T>
|
||||
class matrix {
|
||||
public:
|
||||
matrix(T x) : x_{std::move(x)} {}
|
||||
|
||||
/* print this value */
|
||||
T x_;
|
||||
}; /*matrix*/
|
||||
|
||||
template<typename T>
|
||||
using vector = matrix<T>;
|
||||
|
||||
template<typename T>
|
||||
inline std::ostream &
|
||||
operator<<(std::ostream & s, matrix<T> const & mat)
|
||||
{
|
||||
s << "[";
|
||||
for(std::uint32_t i = 0, m = mat.x_.rows(); i<m; ++i) {
|
||||
if(i > 0)
|
||||
s << "; ";
|
||||
|
||||
for(std::uint32_t j = 0, n = mat.x_.cols(); j<n; ++j) {
|
||||
if(j > 0)
|
||||
s << ' ';
|
||||
|
||||
s << mat.x_(i, j);
|
||||
}
|
||||
}
|
||||
s << "]";
|
||||
|
||||
return s;
|
||||
} /*operator<<*/
|
||||
} /*namespace logutil*/
|
||||
|
||||
/* end print_eigen.hpp */
|
||||
30
xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt
Normal file
30
xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt
Normal file
|
|
@ -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)
|
||||
157
xo-kalmanfilter/src/kalmanfilter/EigenUtil.cpp
Normal file
157
xo-kalmanfilter/src/kalmanfilter/EigenUtil.cpp
Normal file
|
|
@ -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 <Eigen/Dense>
|
||||
#include <memory>
|
||||
#include <cstdint>
|
||||
#include <cassert>
|
||||
|
||||
namespace xo {
|
||||
using xo::json::PrintJson;
|
||||
using xo::json::JsonPrinter;
|
||||
using xo::reflect::Reflect;
|
||||
using xo::reflect::TypeDescr;
|
||||
using VectorXb = Eigen::Array<bool, Eigen::Dynamic, 1>;
|
||||
using Eigen::VectorXd;
|
||||
using Eigen::MatrixXd;
|
||||
|
||||
#ifdef NOT_YET
|
||||
namespace reflect {
|
||||
template<typename ElementType>
|
||||
using EigenVectorX_Tdx = xo::reflect::StlVectorTdx<Eigen::Array<ElementType,
|
||||
Eigen::Dynamic,
|
||||
1>>;
|
||||
|
||||
/* probably need this to appear before decl for class xo::reflect::Reflect */
|
||||
template<typename ElementType>
|
||||
class EstablishTdx<Eigen::Array<ElementType, Eigen::Dynamic, 1>> {
|
||||
public:
|
||||
static std::unique_ptr<TypeDescrExtra> make() {
|
||||
return EigenVectorX_Tdx<ElementType>::make();
|
||||
} /*make*/
|
||||
}; /*EstablishTdx*/
|
||||
} /*reflect*/
|
||||
#endif
|
||||
|
||||
namespace eigen {
|
||||
|
||||
namespace {
|
||||
/* prints a VectorXd as json, in the obvious format, e.g.
|
||||
* [1,2,3]
|
||||
*/
|
||||
template<typename EigenVectorType>
|
||||
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<EigenVectorType>(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<MatrixXd>(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<m; ++i) {
|
||||
if (i > 0)
|
||||
*p_os << ", ";
|
||||
*p_os << "[";
|
||||
for(std::uint32_t j=0, n=pm->cols(); j<n; ++j) {
|
||||
if (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<typename EigenVectorType>
|
||||
void
|
||||
provide_eigen_vector_printer(PrintJson * p_pjson)
|
||||
{
|
||||
TypeDescr td = Reflect::require<EigenVectorType>();
|
||||
std::unique_ptr<JsonPrinter> pr(new EigenVectorJsonPrinter<EigenVectorType>(p_pjson));
|
||||
|
||||
p_pjson->provide_printer(td, std::move(pr));
|
||||
} /*provide_eigen_vector_printer*/
|
||||
} /*namespace*/
|
||||
|
||||
void
|
||||
EigenUtil::reflect_eigen()
|
||||
{
|
||||
#ifdef NOT_YET
|
||||
Reflect::require<VectorXb>();
|
||||
Reflect::require<VectorXd>();
|
||||
#endif
|
||||
} /*reflect_eigen*/
|
||||
|
||||
void
|
||||
EigenUtil::provide_json_printers(PrintJson * p_pjson)
|
||||
{
|
||||
assert(p_pjson);
|
||||
|
||||
provide_eigen_vector_printer<VectorXb>(p_pjson);
|
||||
provide_eigen_vector_printer<VectorXd>(p_pjson);
|
||||
|
||||
{
|
||||
TypeDescr td = Reflect::require<MatrixXd>();
|
||||
std::unique_ptr<JsonPrinter> pr(new MatrixXdJsonPrinter(p_pjson));
|
||||
|
||||
p_pjson->provide_printer(td, std::move(pr));
|
||||
}
|
||||
} /*provide_json_printers*/
|
||||
} /*namespace eigen*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end EigenUtil.cpp */
|
||||
78
xo-kalmanfilter/src/kalmanfilter/KalmanFilter.cpp
Normal file
78
xo-kalmanfilter/src/kalmanfilter/KalmanFilter.cpp
Normal file
|
|
@ -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<KalmanFilterInput> 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 << "<KalmanFilter";
|
||||
os << xtag("filter_spec", filter_spec_);
|
||||
//os << xtag("step", step_);
|
||||
os << xtag("state_ext", state_ext_);
|
||||
os << ">";
|
||||
} /*display*/
|
||||
|
||||
std::string
|
||||
KalmanFilter::display_string() const
|
||||
{
|
||||
return tostr(*this);
|
||||
} /*display_string*/
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilter.cpp */
|
||||
360
xo-kalmanfilter/src/kalmanfilter/KalmanFilterEngine.cpp
Normal file
360
xo-kalmanfilter/src/kalmanfilter/KalmanFilterEngine.cpp
Normal file
|
|
@ -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<KalmanFilterState>
|
||||
KalmanFilterEngine::extrapolate(utc_nanos tkp1,
|
||||
rp<KalmanFilterState> 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<KalmanFilterState> 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<KalmanFilterState> 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<MatrixXd> 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<KalmanFilterStateExt>
|
||||
KalmanFilterEngine::correct1(rp<KalmanFilterState> const & skp1_ext,
|
||||
KalmanFilterObservable const & h,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt>
|
||||
KalmanFilterEngine::correct(rp<KalmanFilterState> const & skp1_ext,
|
||||
KalmanFilterObservable const & h,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt>
|
||||
KalmanFilterEngine::step(utc_nanos tkp1,
|
||||
rp<KalmanFilterState> const & sk,
|
||||
KalmanFilterTransition const & Fk,
|
||||
KalmanFilterObservable const & Hkp1,
|
||||
rp<KalmanFilterInput> const & zkp1)
|
||||
{
|
||||
rp<KalmanFilterState> skp1_ext
|
||||
= KalmanFilterEngine::extrapolate(tkp1, sk, Fk);
|
||||
|
||||
rp<KalmanFilterStateExt> skp1
|
||||
= KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1);
|
||||
|
||||
return skp1;
|
||||
} /*step*/
|
||||
|
||||
rp<KalmanFilterStateExt>
|
||||
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<KalmanFilterStateExt>
|
||||
KalmanFilterEngine::step1(utc_nanos tkp1,
|
||||
rp<KalmanFilterState> const & sk,
|
||||
KalmanFilterTransition const & Fk,
|
||||
KalmanFilterObservable const & Hkp1,
|
||||
rp<KalmanFilterInput> const & zkp1,
|
||||
uint32_t j)
|
||||
{
|
||||
rp<KalmanFilterState> skp1_ext
|
||||
= KalmanFilterEngine::extrapolate(tkp1, sk, Fk);
|
||||
|
||||
rp<KalmanFilterStateExt> skp1
|
||||
= KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j);
|
||||
|
||||
return skp1;
|
||||
} /*step1*/
|
||||
|
||||
rp<KalmanFilterStateExt>
|
||||
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 */
|
||||
118
xo-kalmanfilter/src/kalmanfilter/KalmanFilterInput.cpp
Normal file
118
xo-kalmanfilter/src/kalmanfilter/KalmanFilterInput.cpp
Normal file
|
|
@ -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>
|
||||
KalmanFilterInput::make(utc_nanos tkp1,
|
||||
VectorXb const & presence,
|
||||
VectorXd const & z,
|
||||
VectorXd const & Rd)
|
||||
{
|
||||
return new KalmanFilterInput(tkp1, presence, z, Rd);
|
||||
} /*make*/
|
||||
|
||||
rp<KalmanFilterInput>
|
||||
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(); j<m; ++j) {
|
||||
if (this->presence_[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(); j<m; ++j) {
|
||||
if (this->presence_[j]) {
|
||||
keep[jstar] = j;
|
||||
++jstar;
|
||||
}
|
||||
}
|
||||
|
||||
log && log("keep", matrix(keep));
|
||||
|
||||
return keep;
|
||||
} /*make_kept_index*/
|
||||
|
||||
void
|
||||
KalmanFilterInput::reflect_self()
|
||||
{
|
||||
StructReflector<KalmanFilterInput> 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 << "<KalmanFilterInput"
|
||||
<< xtag("tkp1", tkp1_)
|
||||
<< xtag("z", matrix(z_))
|
||||
<< xtag("presence", matrix(presence_))
|
||||
<< xtag("Rd", matrix(Rd_))
|
||||
<< ">";
|
||||
} /*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 */
|
||||
|
|
@ -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>
|
||||
KalmanFilterInputToConsole::make() {
|
||||
return new KalmanFilterInputToConsole();
|
||||
} /*make*/
|
||||
|
||||
void
|
||||
KalmanFilterInputToConsole::display(std::ostream & os) const
|
||||
{
|
||||
os << "<KalmanFilterInputToConsole"
|
||||
<< xtag("this", (void*)this)
|
||||
<< ">";
|
||||
} /*display*/
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterInputToConsole.cpp */
|
||||
71
xo-kalmanfilter/src/kalmanfilter/KalmanFilterObservable.cpp
Normal file
71
xo-kalmanfilter/src/kalmanfilter/KalmanFilterObservable.cpp
Normal file
|
|
@ -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<H.rows(); ++j)
|
||||
keep[j] = j;
|
||||
|
||||
return KalmanFilterObservable(std::move(keep),
|
||||
std::move(H),
|
||||
std::move(R));
|
||||
} /*keep_all*/
|
||||
|
||||
KalmanFilterObservable
|
||||
KalmanFilterObservable::reindex(VectorXi keep,
|
||||
MatrixXd H,
|
||||
MatrixXd R)
|
||||
{
|
||||
scope log(XO_DEBUG(false /*debug_flag*/));
|
||||
|
||||
/* Hp:
|
||||
* - keep rows in H with indices that appear in keep[]
|
||||
* - keep all columns of H
|
||||
*/
|
||||
|
||||
MatrixXd Hp = H(keep, Eigen::all);
|
||||
MatrixXd Rp = R(keep, keep);
|
||||
|
||||
if (log.enabled()) {
|
||||
log(xtag("keep", matrix(keep)));
|
||||
log(xtag("H", matrix(H)));
|
||||
log(xtag("R", matrix(R)));
|
||||
}
|
||||
|
||||
return KalmanFilterObservable(keep, Hp, Rp);
|
||||
} /*reindex*/
|
||||
|
||||
void
|
||||
KalmanFilterObservable::display(std::ostream & os) const
|
||||
{
|
||||
os << "<KalmanFilterObservable"
|
||||
<< xtag("H", matrix(H_))
|
||||
<< xtag("R", matrix(R_))
|
||||
<< ">";
|
||||
} /*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 */
|
||||
27
xo-kalmanfilter/src/kalmanfilter/KalmanFilterSpec.cpp
Normal file
27
xo-kalmanfilter/src/kalmanfilter/KalmanFilterSpec.cpp
Normal file
|
|
@ -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 << "<KalmanFilterSpec"
|
||||
<< xtag("start_ext", start_ext_)
|
||||
<< ">";
|
||||
} /*display*/
|
||||
|
||||
std::string
|
||||
KalmanFilterSpec::display_string() const
|
||||
{
|
||||
return tostr(*this);
|
||||
} /*display_string*/
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterSpec.cpp */
|
||||
231
xo-kalmanfilter/src/kalmanfilter/KalmanFilterState.cpp
Normal file
231
xo-kalmanfilter/src/kalmanfilter/KalmanFilterState.cpp
Normal file
|
|
@ -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 <ostream>
|
||||
#include <string>
|
||||
|
||||
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>
|
||||
KalmanFilterState::make()
|
||||
{
|
||||
return new KalmanFilterState();
|
||||
} /*make*/
|
||||
|
||||
rp<KalmanFilterState>
|
||||
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<KalmanFilterState> 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>
|
||||
KalmanFilterStateExt::make()
|
||||
{
|
||||
return new KalmanFilterStateExt();
|
||||
} /*make*/
|
||||
|
||||
rp<KalmanFilterStateExt>
|
||||
KalmanFilterStateExt::make(uint32_t k,
|
||||
utc_nanos tk,
|
||||
VectorXd x,
|
||||
MatrixXd P,
|
||||
KalmanFilterTransition transition,
|
||||
MatrixXd K,
|
||||
int32_t j,
|
||||
rp<KalmanFilterInput> 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<KalmanFilterStateExt> sr;
|
||||
|
||||
if (sr.is_incomplete()) {
|
||||
/* TODO: use sr.adopt_ancestors<KalmanFilterState>() */
|
||||
|
||||
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<KalmanFilterInput> 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 << "<KalmanFilterState"
|
||||
<< xtag("k", k_)
|
||||
<< xtag("tk", tk_)
|
||||
<< xtag("x", matrix(x_))
|
||||
<< xtag("P", matrix(P_))
|
||||
<< ">";
|
||||
} /*display*/
|
||||
|
||||
std::string
|
||||
KalmanFilterState::display_string() const
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << *this;
|
||||
return ss.str();
|
||||
} /*display_string*/
|
||||
|
||||
// ----- KalmanFilterStateExt -----
|
||||
|
||||
rp<KalmanFilterStateExt>
|
||||
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 << "<KalmanFilterStateExt"
|
||||
<< xtag("k", this->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 */
|
||||
|
|
@ -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>
|
||||
KalmanFilterStateToConsole::make() {
|
||||
return new KalmanFilterStateToConsole();
|
||||
} /*make*/
|
||||
|
||||
void
|
||||
KalmanFilterStateToConsole::display(std::ostream & os) const
|
||||
{
|
||||
os << "<KalmanFilterStateToConsole"
|
||||
<< xtag("this", (void*)this)
|
||||
<< ">";
|
||||
} /*display*/
|
||||
} /*namespace kalman*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end KalmanFilterStateToConsole.cpp */
|
||||
84
xo-kalmanfilter/src/kalmanfilter/KalmanFilterStep.cpp
Normal file
84
xo-kalmanfilter/src/kalmanfilter/KalmanFilterStep.cpp
Normal file
|
|
@ -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<KalmanFilterState>
|
||||
KalmanFilterStep::extrapolate() const
|
||||
{
|
||||
return KalmanFilterEngine::extrapolate(this->tkp1(),
|
||||
this->state(),
|
||||
this->model() /*transition*/);
|
||||
} /*extrapolate*/
|
||||
|
||||
MatrixXd
|
||||
KalmanFilterStep::gain(rp<KalmanFilterState> const & skp1_ext) const
|
||||
{
|
||||
return KalmanFilterEngine::kalman_gain(skp1_ext,
|
||||
this->obs());
|
||||
} /*gain*/
|
||||
|
||||
VectorXd
|
||||
KalmanFilterStep::gain1(rp<KalmanFilterState> const & skp1_ext,
|
||||
uint32_t j) const
|
||||
{
|
||||
return KalmanFilterEngine::kalman_gain1(skp1_ext,
|
||||
this->obs(),
|
||||
j);
|
||||
|
||||
} /*gain1*/
|
||||
|
||||
rp<KalmanFilterStateExt>
|
||||
KalmanFilterStep::correct(rp<KalmanFilterState> const & skp1_ext)
|
||||
{
|
||||
return KalmanFilterEngine::correct(skp1_ext,
|
||||
this->obs(),
|
||||
this->input());
|
||||
} /*correct*/
|
||||
|
||||
rp<KalmanFilterStateExt>
|
||||
KalmanFilterStep::correct1(rp<KalmanFilterState> 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 << "<KalmanFilterStep";
|
||||
//lscope.log("state:");
|
||||
os << xtag("state", state_);
|
||||
//lscope.log("model:");
|
||||
os << xtag("model", this->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 */
|
||||
44
xo-kalmanfilter/src/kalmanfilter/KalmanFilterSvc.cpp
Normal file
44
xo-kalmanfilter/src/kalmanfilter/KalmanFilterSvc.cpp
Normal file
|
|
@ -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>
|
||||
KalmanFilterSvc::make(KalmanFilterSpec spec)
|
||||
{
|
||||
return new KalmanFilterSvc(std::move(spec));
|
||||
} /*make*/
|
||||
|
||||
KalmanFilterSvc::KalmanFilterSvc(KalmanFilterSpec spec)
|
||||
: filter_{std::move(spec)}
|
||||
{}
|
||||
|
||||
void
|
||||
KalmanFilterSvc::notify_ev(rp<KalmanFilterInput> 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 << "<KalmanFilterSvc"
|
||||
<< xtag("name", this->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 */
|
||||
55
xo-kalmanfilter/src/kalmanfilter/KalmanFilterTransition.cpp
Normal file
55
xo-kalmanfilter/src/kalmanfilter/KalmanFilterTransition.cpp
Normal file
|
|
@ -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<KalmanFilterTransition> 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 << "<KalmanFilterTransition"
|
||||
<< xtag("F", matrix(F_))
|
||||
<< xtag("Q", matrix(Q_))
|
||||
<< ">";
|
||||
} /*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 */
|
||||
52
xo-kalmanfilter/src/kalmanfilter/init_filter.cpp
Normal file
52
xo-kalmanfilter/src/kalmanfilter/init_filter.cpp
Normal file
|
|
@ -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<S_kalmanfilter_tag>::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<S_kalmanfilter_tag>::require()
|
||||
{
|
||||
InitEvidence retval;
|
||||
|
||||
/* subsystem dependencies for filter/ */
|
||||
retval ^= InitSubsys<S_reactor_tag>::require();
|
||||
|
||||
/* filter/'s own initialization code */
|
||||
retval ^= Subsystem::provide<S_kalmanfilter_tag>("kalmanfilter", &init);
|
||||
|
||||
return retval;
|
||||
} /*require*/
|
||||
} /*namespace xo*/
|
||||
|
||||
/* end init_filter.cpp */
|
||||
23
xo-kalmanfilter/utest/CMakeLists.txt
Normal file
23
xo-kalmanfilter/utest/CMakeLists.txt
Normal file
|
|
@ -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
|
||||
687
xo-kalmanfilter/utest/KalmanFilter.test.cpp
Normal file
687
xo-kalmanfilter/utest/KalmanFilter.test.cpp
Normal file
|
|
@ -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 <catch2/catch.hpp>
|
||||
#include <fstream>
|
||||
|
||||
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<KalmanFilterState> const & sk,
|
||||
rp<KalmanFilterInput> 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<xoshiro256ss>::make
|
||||
(seed,
|
||||
std::normal_distribution<double>(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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
|
||||
= KalmanFilterInput::make_present(tkp1, z);
|
||||
|
||||
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
|
||||
|
||||
rp<KalmanFilterStateExt> 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<xoshiro256ss>::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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
|
||||
= KalmanFilterInput::make_present(tkp1, z);
|
||||
|
||||
KalmanFilterStep step_spec
|
||||
= spec.make_step(sk, inputk);
|
||||
|
||||
rp<KalmanFilterStateExt> 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<KalmanFilterState> const & sk,
|
||||
rp<KalmanFilterInput> 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<xoshiro256ss>::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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
|
||||
= KalmanFilterInput::make_present(tkp1, z);
|
||||
|
||||
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
|
||||
|
||||
rp<KalmanFilterStateExt> 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<KalmanFilterState> const & sk,
|
||||
rp<KalmanFilterInput> 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<xoshiro256ss>::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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
|
||||
= KalmanFilterInput::make_present(tkp1, z);
|
||||
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
|
||||
rp<KalmanFilterStateExt> 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 */
|
||||
6
xo-kalmanfilter/utest/filter_utest_main.cpp
Normal file
6
xo-kalmanfilter/utest/filter_utest_main.cpp
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
/* @file filter_utest_main.cpp */
|
||||
|
||||
#define CATCH_CONFIG_MAIN
|
||||
#include "catch2/catch.hpp"
|
||||
|
||||
/* end filter_utest_main.cpp */
|
||||
100
xo-kalmanfilter/utest/utestdata/filter/kalman-revert1
Normal file
100
xo-kalmanfilter/utest/utestdata/filter/kalman-revert1
Normal file
|
|
@ -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
|
||||
Loading…
Add table
Add a link
Reference in a new issue