19 #ifndef SNARK_FILTER_CONSTANT_POSITION_H
20 #define SNARK_FILTER_CONSTANT_POSITION_H
25 namespace constant_position {
29 state(): covariance( Eigen::Matrix< double, dimension, dimension >::Identity() ) {}
31 void set_innovation(
const Eigen::Vector3d& innovation )
33 position += innovation;
36 static const unsigned int dimension = 3u;
37 Eigen::Vector3d position;
38 Eigen::Matrix3d covariance;
44 Eigen::Matrix< double, state::dimension, 1 > sigma;
45 Eigen::Matrix< double, state::dimension, state::dimension >
m_jacobian;
46 Eigen::Matrix< double, state::dimension, state::dimension > noise;
48 model(
void): sigma( Eigen::Matrix< double, state::dimension, 1 >::Ones() ),
49 m_jacobian( Eigen::Matrix< double, state::dimension, state::dimension >::Identity() ),
50 noise( Eigen::Matrix< double, state::dimension, state::dimension >::Zero() )
53 Eigen::Matrix< double, state::dimension, state::dimension > & jacobian(
const state & state,
double dt )
58 void update_state( state & state,
const double dt ){ }
60 Eigen::Matrix< double, state::dimension, state::dimension >& noise_covariance(
double dt )
62 for(
unsigned int i = 0; i < 3; i++ )
64 noise(i,i) = dt * sigma[i];
73 static const int dimension = 3;
74 Eigen::Vector3d position;
75 Eigen::Matrix3d covariance;
76 Eigen::Matrix3d jacobian;
78 position(): position( Eigen::Vector3d::Zero() ),
79 covariance( Eigen::Matrix3d::Identity() ),
80 jacobian( Eigen::Matrix3d::Identity() )
84 const Eigen::Matrix3d& measurement_jacobian(
const state & state )
const
89 const Eigen::Matrix3d measurement_covariance(
const state & state )
const
94 const Eigen::Matrix< double, dimension, 1 > innovation(
const state & state )
const
103 #endif // SNARK_FILTER_CONSTANT_POSITION_H