roland/xo-alloc
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

xo-kalmanfilter: bugfix: track xo::ref::rp -> xo::rp

Browse source
This commit is contained in:
Roland Conybeare 2024-09-14 19:15:47 -05:00
commit 2ced8429c0
20 changed files with 100 additions and 100 deletions
Download patch file Download diff file Expand all files Collapse all files

6
include/xo/kalmanfilter/KalmanFilter.hpp
View file

@ -85,7 +85,7 @@ namespace xo {
utc_nanos tm() const { return state_ext_->tm(); }
KalmanFilterSpec const & filter_spec() const { return filter_spec_; }
KalmanFilterStep const & step() const { return step_; }
ref::rp<KalmanFilterStateExt> const & state_ext() const { return state_ext_; }
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()
@ -95,7 +95,7 @@ namespace xo {
* - .filter_spec_k, .step_k, .state_k updated
* for observations in input_kp1
*/
void notify_input(ref::rp<KalmanFilterInput> const & input_kp1);
void notify_input(rp<KalmanFilterInput> const & input_kp1);
void display(std::ostream & os) const;
std::string display_string() const;
@ -112,7 +112,7 @@ namespace xo {
/* filter state as of most recent observation;
* result of applying KalmanFilterEngine::step() to contents of .step
*/
ref::rp<KalmanFilterStateExt> state_ext_;
rp<KalmanFilterStateExt> state_ext_;
}; /*KalmanFilter*/
inline std::ostream &

32
include/xo/kalmanfilter/KalmanFilterEngine.hpp
View file

@ -39,8 +39,8 @@ namespace xo {
* retval.x = x(k+1|k)
* retval.P = P(k+1|k)
*/
static ref::rp<KalmanFilterState> extrapolate(utc_nanos tkp1,
ref::rp<KalmanFilterState> const & sk,
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)
@ -59,7 +59,7 @@ namespace xo {
* Hkp1. relates model state to observable variables,
* for step t(k) -> t(k+1)
*/
static MatrixXd kalman_gain(ref::rp<KalmanFilterState> const & skp1_ext,
static MatrixXd kalman_gain(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & Hkp1);
/* compute kalman gain for a single observation z(k)[j].
@ -80,7 +80,7 @@ namespace xo {
* amount of innovation in observable #j, i.e. for difference between
* expected and actual value for that observable.
*/
static VectorXd kalman_gain1(ref::rp<KalmanFilterState> const & skp1_ext,
static VectorXd kalman_gain1(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & Hkp1,
uint32_t j);
@ -98,9 +98,9 @@ namespace xo {
*
* return new filter state+cov
*/
static ref::rp<KalmanFilterStateExt> correct(ref::rp<KalmanFilterState> const & skp1_ext,
static rp<KalmanFilterStateExt> correct(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & Hkp1,
ref::rp<KalmanFilterInput> const & zkp1);
rp<KalmanFilterInput> const & zkp1);
/* correct extrapolated filter state for observation
* of j'th filter observable z(k+1)[j]
@ -108,9 +108,9 @@ namespace xo {
* Can use this when observation errors are uncorrelated
* (i.e. observation error matrix R is diagonal)
*/
static ref::rp<KalmanFilterStateExt> correct1(ref::rp<KalmanFilterState> const & skp1_ext,
static rp<KalmanFilterStateExt> correct1(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & Hkp1,
ref::rp<KalmanFilterInput> const & zkp1,
rp<KalmanFilterInput> const & zkp1,
uint32_t j);
/* step filter from t(k) -> t(k+1)
@ -123,11 +123,11 @@ namespace xo {
* H (coupling matrix), R (error covar matrix)
* zkp1. observations z(k+1) for time t(k+1)
*/
static ref::rp<KalmanFilterStateExt> step(utc_nanos tkp1,
ref::rp<KalmanFilterState> const & sk,
static rp<KalmanFilterStateExt> step(utc_nanos tkp1,
rp<KalmanFilterState> const & sk,
KalmanFilterTransition const & Fk,
KalmanFilterObservable const & Hkp1,
ref::rp<KalmanFilterInput> const & zkp1);
rp<KalmanFilterInput> const & zkp1);
/* step filter from t(k) -> tk(k+1)
* same as
@ -136,7 +136,7 @@ namespace xo {
* step_spec. encapsulates Fk (transition-related params)
* and Q (system noise covar matrix)
*/
static ref::rp<KalmanFilterStateExt> step(KalmanFilterStep const & step_spec);
static rp<KalmanFilterStateExt> step(KalmanFilterStep const & step_spec);
/* step filter from t(k) -> t(k+1)
*
@ -150,11 +150,11 @@ namespace xo {
* j. identifies a single filter observable --
* step will only consume observation z(k+1)[j]
*/
static ref::rp<KalmanFilterStateExt> step1(utc_nanos tkp1,
ref::rp<KalmanFilterState> const & sk,
static rp<KalmanFilterStateExt> step1(utc_nanos tkp1,
rp<KalmanFilterState> const & sk,
KalmanFilterTransition const & Fk,
KalmanFilterObservable const & Hkp1,
ref::rp<KalmanFilterInput> const & zkp1,
rp<KalmanFilterInput> const & zkp1,
uint32_t j);
/* step filter from t(k) -> t(k+1)
@ -176,7 +176,7 @@ namespace xo {
* j. identifies a single filter observable --
* step will only consume observation z(k+1)[j]
*/
static ref::rp<KalmanFilterStateExt> step1(KalmanFilterStep const & step_spec,
static rp<KalmanFilterStateExt> step1(KalmanFilterStep const & step_spec,
uint32_t j);
}; /*KalmanFilterEngine*/
} /*namespace kalman*/

6
include/xo/kalmanfilter/KalmanFilterInput.hpp
View file

@ -32,13 +32,13 @@ namespace xo {
public:
KalmanFilterInput() = default;
static ref::rp<KalmanFilterInput> make(utc_nanos tkp1,
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 ref::rp<KalmanFilterInput> make_present(utc_nanos tkp1,
static rp<KalmanFilterInput> make_present(utc_nanos tkp1,
VectorXd const & z);
/* reflect KalmanFilterInput object representation */
@ -106,7 +106,7 @@ namespace xo {
VectorXd Rd_;
}; /*KalmanFilterInput*/
using KalmanFilterInputPtr = ref::rp<KalmanFilterInput>;
using KalmanFilterInputPtr = rp<KalmanFilterInput>;
inline std::ostream &
operator<<(std::ostream & os, KalmanFilterInput const & x)

2
include/xo/kalmanfilter/KalmanFilterInputCallback.hpp
View file

@ -7,7 +7,7 @@
namespace xo {
namespace kalman {
using KalmanFilterInputCallback = reactor::Sink1<ref::rp<KalmanFilterInput>>;
using KalmanFilterInputCallback = reactor::Sink1<rp<KalmanFilterInput>>;
} /*namespace kalman*/
} /*namespace xo*/

4
include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp
View file

@ -8,12 +8,12 @@
namespace xo {
namespace kalman {
class KalmanFilterInputToConsole
: public xo::reactor::SinkToConsole<ref::rp<KalmanFilterInput>>
: public xo::reactor::SinkToConsole<rp<KalmanFilterInput>>
{
public:
KalmanFilterInputToConsole() = default;
static ref::rp<KalmanFilterInputToConsole> make();
static rp<KalmanFilterInputToConsole> make();
virtual void display(std::ostream & os) const;
//virtual std::string display_string() const;

2
include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp
View file

@ -8,7 +8,7 @@
namespace xo {
namespace kalman {
/* callback for consuming kalman filter output */
using KalmanFilterOutputCallback = reactor::Sink1<ref::rp<KalmanFilterStateExt>>;
using KalmanFilterOutputCallback = reactor::Sink1<rp<KalmanFilterStateExt>>;
} /*namespace kalman*/
} /*namespace xo*/

12
include/xo/kalmanfilter/KalmanFilterSpec.hpp
View file

@ -40,15 +40,15 @@ namespace xo {
* }
*/
using MkStepFn = std::function<KalmanFilterStep
(ref::rp<KalmanFilterState> const & sk,
(rp<KalmanFilterState> const & sk,
KalmanFilterInputPtr const & zkp1)>;
public:
explicit KalmanFilterSpec(ref::rp<KalmanFilterStateExt> s0, MkStepFn mkstepfn)
explicit KalmanFilterSpec(rp<KalmanFilterStateExt> s0, MkStepFn mkstepfn)
: start_ext_{std::move(s0)},
mk_step_fn_{std::move(mkstepfn)} {}
ref::rp<KalmanFilterStateExt> const & start_ext() const { return start_ext_; }
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).
*
@ -56,8 +56,8 @@ namespace xo {
* - to allow time stepping to be observation-driven
* - to allow for selective outlier removal
*/
KalmanFilterStep make_step(ref::rp<KalmanFilterState> const & sk,
ref::rp<KalmanFilterInput> const & zkp1) {
KalmanFilterStep make_step(rp<KalmanFilterState> const & sk,
rp<KalmanFilterInput> const & zkp1) {
return this->mk_step_fn_(sk, zkp1);
} /*make_step*/
@ -66,7 +66,7 @@ namespace xo {
private:
/* starting state */
ref::rp<KalmanFilterStateExt> start_ext_;
rp<KalmanFilterStateExt> start_ext_;
/* creates kalman filter step object on demand;
* step object specifies inputs to 1 step in discrete

18
include/xo/kalmanfilter/KalmanFilterState.hpp
View file

@ -24,8 +24,8 @@ namespace xo {
using uint32_t = std::uint32_t;
public:
static ref::rp<KalmanFilterState> make();
static ref::rp<KalmanFilterState> make(uint32_t k,
static rp<KalmanFilterState> make();
static rp<KalmanFilterState> make(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
@ -100,18 +100,18 @@ namespace xo {
using int32_t = std::int32_t;
public:
static ref::rp<KalmanFilterStateExt> make();
static ref::rp<KalmanFilterStateExt> make(uint32_t k,
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,
ref::rp<KalmanFilterInput> zk);
rp<KalmanFilterInput> zk);
/* create state object for initial filter state */
static ref::rp<KalmanFilterStateExt> initial(utc_nanos t0,
static rp<KalmanFilterStateExt> initial(utc_nanos t0,
VectorXd x0,
MatrixXd P0);
@ -120,7 +120,7 @@ namespace xo {
int32_t observable() const { return j_; }
MatrixXd const & gain() const { return K_; }
ref::rp<KalmanFilterInput> const & zk() const { return zk_; }
rp<KalmanFilterInput> const & zk() const { return zk_; }
virtual void display(std::ostream & os) const override;
@ -137,7 +137,7 @@ namespace xo {
KalmanFilterTransition transition,
MatrixXd K,
int32_t j,
ref::rp<KalmanFilterInput> zk);
rp<KalmanFilterInput> zk);
private:
/* if -1: not used;
@ -155,7 +155,7 @@ namespace xo {
/* input leading to state k.
* empty for initial state (i.e. when .k is 0)
*/
ref::rp<KalmanFilterInput> zk_;
rp<KalmanFilterInput> zk_;
}; /*KalamnFilterStateExt*/
} /*namespace filter*/
} /*namespace xo*/

2
include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp
View file

@ -13,7 +13,7 @@ namespace xo {
public:
KalmanFilterStateToConsole() = default;
static ref::rp<KalmanFilterStateToConsole> make();
static rp<KalmanFilterStateToConsole> make();
virtual void display(std::ostream & os) const;
//virtual std::string display_string() const;

20
include/xo/kalmanfilter/KalmanFilterStep.hpp
View file

@ -55,16 +55,16 @@ namespace xo {
public:
KalmanFilterStep() = default;
KalmanFilterStep(ref::rp<KalmanFilterState> state,
KalmanFilterStep(rp<KalmanFilterState> state,
KalmanFilterTransition model,
KalmanFilterObservable obs,
ref::rp<KalmanFilterInput> zkp1)
rp<KalmanFilterInput> zkp1)
: KalmanFilterStepBase(model, obs),
state_{std::move(state)},
input_{std::move(zkp1)} {}
ref::rp<KalmanFilterState> const & state() const { return state_; }
ref::rp<KalmanFilterInput> const & input() const { return input_; }
rp<KalmanFilterState> const & state() const { return state_; }
rp<KalmanFilterInput> const & input() const { return input_; }
utc_nanos tkp1() const { return input_->tkp1(); }
@ -74,14 +74,14 @@ namespace xo {
* P(k+1|k)
* does not use the t(k+1) observations .input.z
*/
ref::rp<KalmanFilterState> extrapolate() const;
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(ref::rp<KalmanFilterState> const & skp1_ext) const;
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)},
@ -90,18 +90,18 @@ namespace xo {
* 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(ref::rp<KalmanFilterState> const & skp1_ext,
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()
*/
ref::rp<KalmanFilterStateExt> correct(ref::rp<KalmanFilterState> const & skp1_ext);
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]
*/
ref::rp<KalmanFilterStateExt> correct1(ref::rp<KalmanFilterState> const & skp1_ext,
rp<KalmanFilterStateExt> correct1(rp<KalmanFilterState> const & skp1_ext,
uint32_t j);
void display(std::ostream & os) const;
@ -111,7 +111,7 @@ namespace xo {
/* system state: timestamp, estimated process state, process covariance
* asof beginning of this step
*/
ref::rp<KalmanFilterState> state_;
rp<KalmanFilterState> state_;
/* input: observations at time t(k+1) */
KalmanFilterInputPtr input_;
}; /*KalmanFilterStep*/

10
include/xo/kalmanfilter/KalmanFilterSvc.hpp
View file

@ -15,19 +15,19 @@ namespace xo {
* sinks that want to consume KalmanFilterSvc events will use
* .attach_sink() (or .add_callback())
*/
class KalmanFilterSvc : public xo::reactor::Sink1<ref::rp<KalmanFilterInput>>,
public xo::reactor::DirectSourcePtr<ref::rp<KalmanFilterStateExt>> {
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 ref::rp<KalmanFilterSvc> make(KalmanFilterSpec spec);
static rp<KalmanFilterSvc> make(KalmanFilterSpec spec);
KalmanFilter const & filter() const { return filter_; }
/* notify incoming observations; will trigger kalman filter step */
void notify_ev(ref::rp<KalmanFilterInput> const & input_kp1) override;
void notify_ev(rp<KalmanFilterInput> const & input_kp1) override;
// ----- inherited from reactor::AbstractSink -----
@ -52,7 +52,7 @@ namespace xo {
/* passive kalman filter */
KalmanFilter filter_;
/* receive filter input from this source; see .attach_input() */
ref::rp<KalmanFilterInputSource> input_src_;
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 */

2
src/kalmanfilter/KalmanFilter.cpp
View file

@ -24,7 +24,7 @@ namespace xo {
{} /*ctor*/
void
KalmanFilter::notify_input(ref::rp<KalmanFilterInput> const & input_kp1)
KalmanFilter::notify_input(rp<KalmanFilterInput> const & input_kp1)
{
scope log(XO_ENTER0(info));

44
src/kalmanfilter/KalmanFilterEngine.cpp
View file

@ -19,9 +19,9 @@ namespace xo {
namespace kalman {
// ----- KalmanFilterEngine -----
ref::rp<KalmanFilterState>
rp<KalmanFilterState>
KalmanFilterEngine::extrapolate(utc_nanos tkp1,
ref::rp<KalmanFilterState> const & s,
rp<KalmanFilterState> const & s,
KalmanFilterTransition const & f)
{
//constexpr char const * c_self_name
@ -60,7 +60,7 @@ namespace xo {
} /*extrapolate*/
VectorXd
KalmanFilterEngine::kalman_gain1(ref::rp<KalmanFilterState> const & skp1_ext,
KalmanFilterEngine::kalman_gain1(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & h,
uint32_t j)
{
@ -110,7 +110,7 @@ namespace xo {
} /*kalman_gain1*/
MatrixXd
KalmanFilterEngine::kalman_gain(ref::rp<KalmanFilterState> const & skp1_ext,
KalmanFilterEngine::kalman_gain(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & h)
{
scope log(XO_DEBUG(false /*debug_enabled*/));
@ -214,10 +214,10 @@ namespace xo {
return K;
} /*kalman_gain*/
ref::rp<KalmanFilterStateExt>
KalmanFilterEngine::correct1(ref::rp<KalmanFilterState> const & skp1_ext,
rp<KalmanFilterStateExt>
KalmanFilterEngine::correct1(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & h,
ref::rp<KalmanFilterInput> const & zkp1,
rp<KalmanFilterInput> const & zkp1,
uint32_t j)
{
uint32_t n = skp1_ext->n_state();
@ -261,10 +261,10 @@ namespace xo {
zkp1);
} /*correct1*/
ref::rp<KalmanFilterStateExt>
KalmanFilterEngine::correct(ref::rp<KalmanFilterState> const & skp1_ext,
rp<KalmanFilterStateExt>
KalmanFilterEngine::correct(rp<KalmanFilterState> const & skp1_ext,
KalmanFilterObservable const & h,
ref::rp<KalmanFilterInput> const & zkp1)
rp<KalmanFilterInput> const & zkp1)
{
uint32_t n = skp1_ext->n_state();
/* K :: [n x m] */
@ -300,23 +300,23 @@ namespace xo {
zkp1);
} /*correct*/
ref::rp<KalmanFilterStateExt>
rp<KalmanFilterStateExt>
KalmanFilterEngine::step(utc_nanos tkp1,
ref::rp<KalmanFilterState> const & sk,
rp<KalmanFilterState> const & sk,
KalmanFilterTransition const & Fk,
KalmanFilterObservable const & Hkp1,
ref::rp<KalmanFilterInput> const & zkp1)
rp<KalmanFilterInput> const & zkp1)
{
ref::rp<KalmanFilterState> skp1_ext
rp<KalmanFilterState> skp1_ext
= KalmanFilterEngine::extrapolate(tkp1, sk, Fk);
ref::rp<KalmanFilterStateExt> skp1
rp<KalmanFilterStateExt> skp1
= KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1);
return skp1;
} /*step*/
ref::rp<KalmanFilterStateExt>
rp<KalmanFilterStateExt>
KalmanFilterEngine::step(KalmanFilterStep const & step_spec)
{
return step(step_spec.tkp1(),
@ -326,24 +326,24 @@ namespace xo {
step_spec.input());
} /*step*/
ref::rp<KalmanFilterStateExt>
rp<KalmanFilterStateExt>
KalmanFilterEngine::step1(utc_nanos tkp1,
ref::rp<KalmanFilterState> const & sk,
rp<KalmanFilterState> const & sk,
KalmanFilterTransition const & Fk,
KalmanFilterObservable const & Hkp1,
ref::rp<KalmanFilterInput> const & zkp1,
rp<KalmanFilterInput> const & zkp1,
uint32_t j)
{
ref::rp<KalmanFilterState> skp1_ext
rp<KalmanFilterState> skp1_ext
= KalmanFilterEngine::extrapolate(tkp1, sk, Fk);
ref::rp<KalmanFilterStateExt> skp1
rp<KalmanFilterStateExt> skp1
= KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j);
return skp1;
} /*step1*/
ref::rp<KalmanFilterStateExt>
rp<KalmanFilterStateExt>
KalmanFilterEngine::step1(KalmanFilterStep const & step_spec,
uint32_t j)
{

4
src/kalmanfilter/KalmanFilterInput.cpp
View file

@ -19,7 +19,7 @@ namespace xo {
using std::uint32_t;
namespace kalman {
ref::rp<KalmanFilterInput>
rp<KalmanFilterInput>
KalmanFilterInput::make(utc_nanos tkp1,
VectorXb const & presence,
VectorXd const & z,
@ -28,7 +28,7 @@ namespace xo {
return new KalmanFilterInput(tkp1, presence, z, Rd);
} /*make*/
ref::rp<KalmanFilterInput>
rp<KalmanFilterInput>
KalmanFilterInput::make_present(utc_nanos tkp1,
VectorXd const & z)
{

2
src/kalmanfilter/KalmanFilterInputToConsole.cpp
View file

@ -7,7 +7,7 @@ namespace xo {
using xo::xtag;
namespace kalman {
ref::rp<KalmanFilterInputToConsole>
rp<KalmanFilterInputToConsole>
KalmanFilterInputToConsole::make() {
return new KalmanFilterInputToConsole();
} /*make*/

4
src/kalmanfilter/KalmanFilterState.cpp
View file

@ -14,7 +14,7 @@ namespace xo {
using xo::reflect::TaggedRcptr;
using xo::reflect::StructReflector;
using xo::time::utc_nanos;
using xo::ref::rp;
using xo::rp;
using logutil::matrix;
using logutil::vector;
//using xo::scope;
@ -190,7 +190,7 @@ namespace xo {
// ----- KalmanFilterStateExt -----
ref::rp<KalmanFilterStateExt>
rp<KalmanFilterStateExt>
KalmanFilterStateExt::initial(utc_nanos t0,
VectorXd x0,
MatrixXd P0)

2
src/kalmanfilter/KalmanFilterStateToConsole.cpp
View file

@ -7,7 +7,7 @@ namespace xo {
using xo::xtag;
namespace kalman {
ref::rp<KalmanFilterStateToConsole>
rp<KalmanFilterStateToConsole>
KalmanFilterStateToConsole::make() {
return new KalmanFilterStateToConsole();
} /*make*/

14
src/kalmanfilter/KalmanFilterStep.cpp
View file

@ -13,7 +13,7 @@ namespace xo {
using Eigen::VectorXd;
namespace kalman {
ref::rp<KalmanFilterState>
rp<KalmanFilterState>
KalmanFilterStep::extrapolate() const
{
return KalmanFilterEngine::extrapolate(this->tkp1(),
@ -22,14 +22,14 @@ namespace xo {
} /*extrapolate*/
MatrixXd
KalmanFilterStep::gain(ref::rp<KalmanFilterState> const & skp1_ext) const
KalmanFilterStep::gain(rp<KalmanFilterState> const & skp1_ext) const
{
return KalmanFilterEngine::kalman_gain(skp1_ext,
this->obs());
} /*gain*/
VectorXd
KalmanFilterStep::gain1(ref::rp<KalmanFilterState> const & skp1_ext,
KalmanFilterStep::gain1(rp<KalmanFilterState> const & skp1_ext,
uint32_t j) const
{
return KalmanFilterEngine::kalman_gain1(skp1_ext,
@ -38,16 +38,16 @@ namespace xo {
} /*gain1*/
ref::rp<KalmanFilterStateExt>
KalmanFilterStep::correct(ref::rp<KalmanFilterState> const & skp1_ext)
rp<KalmanFilterStateExt>
KalmanFilterStep::correct(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,
rp<KalmanFilterStateExt>
KalmanFilterStep::correct1(rp<KalmanFilterState> const & skp1_ext,
uint32_t j)
{
return KalmanFilterEngine::correct1(skp1_ext,

4
src/kalmanfilter/KalmanFilterSvc.cpp
View file

@ -3,7 +3,7 @@
#include "KalmanFilterSvc.hpp"
namespace xo {
using xo::ref::rp;
using xo::rp;
using xo::scope;
using xo::xtag;
@ -19,7 +19,7 @@ namespace xo {
{}
void
KalmanFilterSvc::notify_ev(ref::rp<KalmanFilterInput> const & input_kp1)
KalmanFilterSvc::notify_ev(rp<KalmanFilterInput> const & input_kp1)
{
this->filter_.notify_input(input_kp1);

10
utest/KalmanFilter.test.cpp
View file

@ -26,7 +26,7 @@ namespace xo {
using xo::time::timeutil;
using xo::time::utc_nanos;
using xo::time::seconds;
using xo::ref::rp;
using xo::rp;
using xo::log_level;
using logutil::matrix;
using xo::print::ccs;
@ -144,7 +144,7 @@ namespace xo {
z_stats.include_sample(z[0]);
ref::rp<KalmanFilterInput> inputk
rp<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
@ -259,7 +259,7 @@ namespace xo {
z_stats.include_sample(z[0]);
ref::rp<KalmanFilterInput> inputk
rp<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec
@ -408,7 +408,7 @@ namespace xo {
INFO(tostr(xtag("i_step", i_step), xtag("z", z)));
ref::rp<KalmanFilterInput> inputk
rp<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
@ -571,7 +571,7 @@ namespace xo {
z_stats.include_sample(z[0]);
ref::rp<KalmanFilterInput> inputk
rp<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
rp<KalmanFilterStateExt> skp1 = KalmanFilterEngine::step(step_spec);