78 lines
2.1 KiB
C++
78 lines
2.1 KiB
C++
/* @file KalmanFilter.cpp */
|
|
|
|
#include "KalmanFilter.hpp"
|
|
#include "KalmanFilterEngine.hpp"
|
|
#include "print_eigen.hpp"
|
|
#include "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(ref::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 */
|