snark
constant_position.h
1 // This file is part of snark, a generic and flexible library
2 // for robotics research.
3 //
4 // Copyright (C) 2011 The University of Sydney
5 //
6 // snark is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // snark is distributed in the hope that it will be useful, but WITHOUT ANY
12 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
13 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
14 // for more details.
15 //
16 // You should have received a copy of the GNU Lesser General Public
17 // License along with snark. If not, see <http://www.gnu.org/licenses/>.
18 
19 #ifndef SNARK_FILTER_CONSTANT_POSITION_H
20 #define SNARK_FILTER_CONSTANT_POSITION_H
21 
22 #include <Eigen/Core>
23 
24 namespace snark{
25 namespace constant_position {
26 
27 struct state
28 {
29  state(): covariance( Eigen::Matrix< double, dimension, dimension >::Identity() ) {}
30 
31  void set_innovation( const Eigen::Vector3d& innovation )
32  {
33  position += innovation;
34  }
35 
36  static const unsigned int dimension = 3u;
37  Eigen::Vector3d position;
38  Eigen::Matrix3d covariance;
39 };
40 
42 struct model
43 {
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;
47 
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() )
51  { }
52 
53  Eigen::Matrix< double, state::dimension, state::dimension > & jacobian( const state & state, double dt )
54  {
55  return m_jacobian;
56  }
57 
58  void update_state( state & state, const double dt ){ }
59 
60  Eigen::Matrix< double, state::dimension, state::dimension >& noise_covariance( double dt )
61  {
62  for( unsigned int i = 0; i < 3; i++ )
63  {
64  noise(i,i) = dt * sigma[i];
65  }
66  return noise;
67  }
68 
69 };
70 
71 struct position
72 {
73  static const int dimension = 3;
74  Eigen::Vector3d position;
75  Eigen::Matrix3d covariance;
76  Eigen::Matrix3d jacobian;
77 
78  position(): position( Eigen::Vector3d::Zero() ),
79  covariance( Eigen::Matrix3d::Identity() ),
80  jacobian( Eigen::Matrix3d::Identity() )
81  {
82  }
83 
84  const Eigen::Matrix3d& measurement_jacobian( const state & state ) const
85  {
86  return jacobian;
87  }
88 
89  const Eigen::Matrix3d measurement_covariance( const state & state ) const
90  {
91  return covariance;
92  }
93 
94  const Eigen::Matrix< double, dimension, 1 > innovation( const state & state ) const
95  {
96  return position;
97  }
98 };
99 
100 
101 } } }
102 
103 #endif // SNARK_FILTER_CONSTANT_POSITION_H