62 lines
1.6 KiB
C++
62 lines
1.6 KiB
C++
/* @file KalmanFilterTransition.hpp */
|
|
|
|
#pragma once
|
|
|
|
#include "time/Time.hpp"
|
|
#include <Eigen/Dense>
|
|
#include <cstdint>
|
|
|
|
namespace xo {
|
|
namespace kalman {
|
|
|
|
/* encapsulate transition behavior for a kalman filter
|
|
* before taking observations into account
|
|
*/
|
|
class KalmanFilterTransition {
|
|
public:
|
|
using MatrixXd = Eigen::MatrixXd;
|
|
using uint32_t = std::uint32_t;
|
|
|
|
public:
|
|
KalmanFilterTransition() = default;
|
|
KalmanFilterTransition(MatrixXd F,
|
|
MatrixXd Q)
|
|
: F_{std::move(F)}, Q_{std::move(Q)} { assert(this->check_ok()); }
|
|
|
|
static void reflect_self();
|
|
|
|
/* n: cardinality of state vector */
|
|
uint32_t n_state() const;
|
|
|
|
MatrixXd const & transition_mat() const { return F_; }
|
|
MatrixXd const & transition_cov() const { return Q_; }
|
|
|
|
bool check_ok() const {
|
|
uint32_t n = F_.rows();
|
|
bool f_is_nxn = ((F_.rows() == n) && (F_.cols() == n));
|
|
bool q_is_nxn = ((Q_.rows() == n) && (Q_.cols() == n));
|
|
|
|
/* also would like to require: Q is +ve definite */
|
|
|
|
return f_is_nxn && q_is_nxn;
|
|
} /*check_ok*/
|
|
|
|
void display(std::ostream & os) const;
|
|
std::string display_string() const;
|
|
|
|
private:
|
|
/* [n x n] state transition matrix */
|
|
MatrixXd F_;
|
|
/* [n x n] covariance matrix for system noise */
|
|
MatrixXd Q_;
|
|
}; /*KalmanFilterTransition*/
|
|
|
|
inline std::ostream &
|
|
operator<<(std::ostream & os, KalmanFilterTransition const & x) {
|
|
x.display(os);
|
|
return os;
|
|
} /*operator<<*/
|
|
} /*namespace kalman*/
|
|
} /*namespace xo*/
|
|
|
|
/* end KalmanFilterTransition.hpp */
|