687 lines
26 KiB
C++
687 lines
26 KiB
C++
/* @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 */
|