118 lines
2.8 KiB
C++
118 lines
2.8 KiB
C++
/* @file KalmanFilterInput.cpp */
|
|
|
|
#include "KalmanFilterInput.hpp"
|
|
#include "reflect/StructReflector.hpp"
|
|
#include "Eigen/src/Core/Matrix.h"
|
|
#include "print_eigen.hpp"
|
|
#include "indentlog/scope.hpp"
|
|
#include "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 {
|
|
ref::rp<KalmanFilterInput>
|
|
KalmanFilterInput::make(utc_nanos tkp1,
|
|
VectorXb const & presence,
|
|
VectorXd const & z,
|
|
VectorXd const & Rd)
|
|
{
|
|
return new KalmanFilterInput(tkp1, presence, z, Rd);
|
|
} /*make*/
|
|
|
|
ref::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 */
|