00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef HECTOR_POSE_ESTIMATION_SYSTEM_MODEL_H
00030 #define HECTOR_POSE_ESTIMATION_SYSTEM_MODEL_H
00031
00032 #include <hector_pose_estimation/model.h>
00033 #include <hector_pose_estimation/substate.h>
00034 #include <hector_pose_estimation/input.h>
00035
00036 namespace hector_pose_estimation {
00037
00038 class SystemModel : public Model {
00039 public:
00040 virtual ~SystemModel() {}
00041
00042 virtual int getDimension() const = 0;
00043 virtual bool isSubSystem() const { return false; }
00044 virtual IndexType getStateIndex(const State&) const { return 0; }
00045
00046 enum SystemTypeEnum { UNKNOWN_SYSTEM_TYPE, TIME_DISCRETE, TIME_CONTINUOUS };
00047 virtual SystemTypeEnum getSystemType() const { return UNKNOWN_SYSTEM_TYPE; }
00048
00049 virtual SystemStatus getStatusFlags(const State& state) { return SystemStatus(0); }
00050 virtual bool active(const State& state) { return true; }
00051
00052 virtual void getPrior(State &state) {}
00053
00054 virtual bool prepareUpdate(State& state, double dt) { return true; }
00055 virtual void afterUpdate(State& state) {}
00056
00057 virtual bool limitState(State& state) { return true; }
00058 };
00059
00060 template <int _SubDimension>
00061 class SubSystemModel_ : public SystemModel {
00062 public:
00063 enum { SubDimension = _SubDimension };
00064
00065 virtual ~SubSystemModel_() {}
00066
00067 virtual bool isSubSystem() const { return true; }
00068 virtual IndexType getStateIndex(const State& state) const { return state.getSubState<SubDimension>(this)->getIndex(); }
00069 };
00070
00071 template <>
00072 class SubSystemModel_<0> : public SystemModel {};
00073
00074 namespace traits {
00075
00076 template <int _SubDimension>
00077 struct SystemModel {
00078 typedef SubState_<_SubDimension> StateType;
00079 enum { BaseDimension = State::Dimension };
00080 enum { SubDimension = _SubDimension };
00081 enum { Dimension = StateType::Dimension };
00082
00083 typedef typename StateType::Vector StateVector;
00084 typedef typename StateType::VectorSegment StateVectorSegment;
00085 typedef typename StateType::CovarianceBlock StateCovarianceBlock;
00086 typedef typename StateType::ConstVectorSegment ConstStateVectorSegment;
00087 typedef typename StateType::ConstCovarianceBlock ConstStateCovarianceBlock;
00088
00089 typedef SymmetricMatrix_<Dimension> NoiseVariance;
00090 typedef Block<typename State::Covariance::Base,Dimension,Dimension> NoiseVarianceBlock;
00091 typedef Block<const typename State::Covariance::Base,Dimension,Dimension> ConstNoiseVarianceBlock;
00092
00093 typedef Matrix_<Dimension,Dimension> SystemMatrix;
00094 typedef Block<Matrix,Dimension,Dimension> SystemMatrixBlock;
00095 typedef Block<const Matrix,Dimension,Dimension> ConstSystemMatrixBlock;
00096
00097 typedef boost::integral_constant<bool,(_SubDimension > 0)> IsSubSystem;
00098 typedef SubState_<SubDimension> SubState;
00099 typedef typename SubState::Ptr SubStatePtr;
00100 typedef Matrix_<BaseDimension,SubDimension> CrossSystemMatrix;
00101 typedef Block<Matrix,BaseDimension,SubDimension> CrossSystemMatrixBlock;
00102 typedef Block<const Matrix,BaseDimension,SubDimension> ConstCrossSystemMatrixBlock;
00103 };
00104
00105 #define SYSTEM_MODEL_TRAIT(Derived, _SubDimension) \
00106 typedef traits::SystemModel<_SubDimension> trait; \
00107 typedef typename trait::StateType StateType; \
00108 enum { BaseDimension = trait::BaseDimension }; \
00109 enum { SubDimension = trait::SubDimension }; \
00110 enum { Dimension = trait::Dimension }; \
00111 \
00112 typedef typename trait::StateVector StateVector; \
00113 typedef typename trait::StateVectorSegment StateVectorSegment; \
00114 typedef typename trait::StateCovarianceBlock StateCovarianceBlock; \
00115 typedef typename trait::ConstStateVectorSegment ConstStateVectorSegment; \
00116 typedef typename trait::ConstStateCovarianceBlock ConstStateCovarianceBlock; \
00117 \
00118 typedef typename trait::NoiseVariance NoiseVariance; \
00119 typedef typename trait::NoiseVarianceBlock NoiseVarianceBlock; \
00120 typedef typename trait::ConstNoiseVarianceBlock ConstNoiseVarianceBlock; \
00121 \
00122 typedef typename trait::SystemMatrix SystemMatrix; \
00123 typedef typename trait::SystemMatrixBlock SystemMatrixBlock; \
00124 typedef typename trait::ConstSystemMatrixBlock ConstSystemMatrixBlock; \
00125 \
00126 enum { InputDimension = traits::Input<Derived>::Dimension }; \
00127 typedef typename traits::Input<Derived>::Type InputType; \
00128 typedef typename traits::Input<Derived>::Vector InputVector; \
00129 typedef Matrix_<Dynamic,InputDimension> InputMatrix; \
00130 typedef Block<typename InputMatrix::Base,Dimension,InputDimension> InputMatrixBlock; \
00131 typedef Block<const typename InputMatrix::Base,Dimension,InputDimension> ConstInputMatrixBlock; \
00132 \
00133 typedef typename trait::IsSubSystem IsSubSystem; \
00134 typedef typename trait::SubState SubState; \
00135 typedef typename trait::SubStatePtr SubStatePtr; \
00136 typedef typename trait::CrossSystemMatrix CrossSystemMatrix; \
00137 typedef typename trait::CrossSystemMatrixBlock CrossSystemMatrixBlock; \
00138 typedef typename trait::ConstCrossSystemMatrixBlock ConstCrossSystemMatrixBlock; \
00139
00140 }
00141
00142 template <class Derived, int _SubDimension>
00143 class SystemModel_ : public SubSystemModel_<_SubDimension> {
00144 public:
00145 SYSTEM_MODEL_TRAIT(Derived, _SubDimension)
00146 virtual ~SystemModel_() {}
00147
00148 int getDimension() const { return trait::Dimension; }
00149 virtual SystemModel::SystemTypeEnum getSystemType() const { return SystemModel::TIME_DISCRETE; }
00150
00151 virtual void getPrior(State &state);
00152
00153 Derived *derived() { return static_cast<Derived *>(this); }
00154 const Derived *derived() const { return static_cast<const Derived *>(this); }
00155
00156 SubState& sub(State& state) const { return *state.getSubState<SubDimension>(this); }
00157 const SubState& sub(const State& state) const { return *state.getSubState<SubDimension>(this); }
00158
00159
00160 virtual void getExpectedValue(StateVectorSegment& x_pred, const State& state, double dt) { x_pred = this->sub(state).getVector(); }
00161 virtual void getStateJacobian(SystemMatrixBlock& A, const State& state, double dt) {}
00162 virtual void getInputJacobian(InputMatrixBlock& B, const State& state, double dt) {}
00163 virtual void getSystemNoise(NoiseVarianceBlock& Q, const State& state, double dt) {}
00164
00165
00166 virtual void getStateJacobian(SystemMatrixBlock& A, const State& state, double dt, bool init) { getStateJacobian(A, state, dt); }
00167 virtual void getInputJacobian(InputMatrixBlock& B, const State& state, double dt, bool init) { getInputJacobian(B, state, dt); }
00168 virtual void getSystemNoise(NoiseVarianceBlock& Q, const State& state, double dt, bool init) { getSystemNoise(Q, state, dt); }
00169
00170
00171 virtual void getStateJacobian(SystemMatrixBlock& A1, CrossSystemMatrixBlock& A01, const State& state, double dt) {}
00172 virtual void getStateJacobian(SystemMatrixBlock& A1, CrossSystemMatrixBlock& A01, const State& state, double dt, bool init) { getStateJacobian(A1, A01, state, dt); }
00173 };
00174
00175 template <class Derived, int _SubDimension = 0>
00176 class TimeDiscreteSystemModel_ : public SystemModel_<Derived, _SubDimension> {
00177 public:
00178 SYSTEM_MODEL_TRAIT(Derived, _SubDimension)
00179 virtual ~TimeDiscreteSystemModel_() {}
00180 };
00181
00182 template <class Derived, int _SubDimension = 0>
00183 class TimeContinuousSystemModel_ : public SystemModel_<Derived, _SubDimension> {
00184 public:
00185 SYSTEM_MODEL_TRAIT(Derived, _SubDimension)
00186
00187 TimeContinuousSystemModel_();
00188 virtual ~TimeContinuousSystemModel_();
00189
00190 virtual SystemModel::SystemTypeEnum getSystemType() const { return SystemModel::TIME_CONTINUOUS; }
00191
00192
00193 virtual void getDerivative(StateVector& x_dot, const State& state) { x_dot.setZero(); }
00194 virtual void getStateJacobian(SystemMatrix& A, const State& state) {}
00195 virtual void getInputJacobian(InputMatrix& B, const State& state) {}
00196 virtual void getSystemNoise(NoiseVariance& Q, const State& state) {}
00197
00198
00199 virtual void getStateJacobian(SystemMatrix& A, const State& state, bool init) { getStateJacobian(A, state); }
00200 virtual void getInputJacobian(InputMatrix& B, const State& state, bool init) { getInputJacobian(B, state); }
00201 virtual void getSystemNoise(NoiseVariance& Q, const State& state, bool init) { getSystemNoise(Q, state); }
00202
00203
00204 virtual void getStateJacobian(SystemMatrix& A1, CrossSystemMatrix& A01, const State& state) {}
00205 virtual void getStateJacobian(SystemMatrix& A1, CrossSystemMatrix& A01, const State& state, bool init) { getStateJacobian(A1, A01, state); }
00206
00207
00208 void getExpectedValue(StateVectorSegment& x_pred, const State& state, double dt);
00209 void getStateJacobian(SystemMatrixBlock& A, const State& state, double dt, bool init);
00210 void getStateJacobian(SystemMatrixBlock& A1, CrossSystemMatrixBlock& A01, const State& state, double dt, bool init);
00211 void getInputJacobian(InputMatrixBlock& B, const State& state, double dt, bool init);
00212 void getSystemNoise(NoiseVarianceBlock& Q, const State& state, double dt, bool init);
00213
00214 private:
00215 struct internal;
00216 struct internal *internal_;
00217 };
00218
00219 }
00220
00221 #include "system_model.inl"
00222
00223 #endif // HECTOR_POSE_ESTIMATION_SYSTEM_MODEL_H