84 lines
2.2 KiB
C++
84 lines
2.2 KiB
C++
/* @file KalmanFilterStep.cpp */
|
|
|
|
#include "KalmanFilterStep.hpp"
|
|
#include "filter/KalmanFilterEngine.hpp"
|
|
#include "filter/KalmanFilterState.hpp"
|
|
#include "indentlog/scope.hpp"
|
|
|
|
namespace xo {
|
|
using xo::scope;
|
|
using xo::tostr;
|
|
using xo::xtag;
|
|
using Eigen::MatrixXd;
|
|
using Eigen::VectorXd;
|
|
|
|
namespace kalman {
|
|
ref::rp<KalmanFilterState>
|
|
KalmanFilterStep::extrapolate() const
|
|
{
|
|
return KalmanFilterEngine::extrapolate(this->tkp1(),
|
|
this->state(),
|
|
this->model() /*transition*/);
|
|
} /*extrapolate*/
|
|
|
|
MatrixXd
|
|
KalmanFilterStep::gain(ref::rp<KalmanFilterState> const & skp1_ext) const
|
|
{
|
|
return KalmanFilterEngine::kalman_gain(skp1_ext,
|
|
this->obs());
|
|
} /*gain*/
|
|
|
|
VectorXd
|
|
KalmanFilterStep::gain1(ref::rp<KalmanFilterState> const & skp1_ext,
|
|
uint32_t j) const
|
|
{
|
|
return KalmanFilterEngine::kalman_gain1(skp1_ext,
|
|
this->obs(),
|
|
j);
|
|
|
|
} /*gain1*/
|
|
|
|
ref::rp<KalmanFilterStateExt>
|
|
KalmanFilterStep::correct(ref::rp<KalmanFilterState> const & skp1_ext)
|
|
{
|
|
return KalmanFilterEngine::correct(skp1_ext,
|
|
this->obs(),
|
|
this->input());
|
|
} /*correct*/
|
|
|
|
ref::rp<KalmanFilterStateExt>
|
|
KalmanFilterStep::correct1(ref::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 */
|