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 bool init(PoseEstimation& estimator, System &system, State& state) { return true; }
00043
00044 enum SystemTypeEnum { UNKNOWN_SYSTEM_TYPE, TIME_DISCRETE, TIME_CONTINUOUS };
00045 virtual SystemTypeEnum getSystemType() const { return UNKNOWN_SYSTEM_TYPE; }
00046
00047 virtual SystemStatus getStatusFlags(const State& state) { return SystemStatus(0); }
00048 virtual bool active(const State& state) { return true; }
00049
00050 virtual void getPrior(State &state) {}
00051
00052 virtual bool prepareUpdate(State& state, double dt) { return true; }
00053 virtual void afterUpdate(State& state) {}
00054
00055 virtual bool limitState(State& state) { return true; }
00056 };
00057
00058 namespace traits {
00059
00060 template <class Derived, int _VectorDimension = Derived::VectorDimension, int _CovarianceDimension = _VectorDimension>
00061 struct SystemModel {
00062 enum { VectorDimension = _VectorDimension };
00063 enum { CovarianceDimension = _CovarianceDimension };
00064
00065 typedef State::Vector StateVector;
00066 typedef State::Covariance NoiseVariance;
00067 typedef State::SystemMatrix SystemMatrix;
00068
00069 enum { InputDimension = traits::Input<Derived>::Dimension };
00070 typedef typename traits::Input<Derived>::Type InputType;
00071 typedef typename traits::Input<Derived>::Vector InputVector;
00072 typedef Matrix_<State::Covariance::RowsAtCompileTime,InputDimension> InputMatrix;
00073
00074 typedef SubState_<VectorDimension,CovarianceDimension> SubState;
00075 typedef ColumnVector_<VectorDimension> Vector;
00076
00077 typedef typename SubState::VectorSegment VectorSegment;
00078 typedef typename SubState::CovarianceBlock CovarianceBlock;
00079 typedef typename SubState::CrossVarianceBlock CrossVarianceBlock;
00080 typedef Block<SystemMatrix,VectorDimension,SystemMatrix::ColsAtCompileTime> SystemMatrixBlock;
00081
00082 typedef typename SubState::ConstVectorSegment ConstVectorSegment;
00083 typedef typename SubState::ConstCovarianceBlock ConstCovarianceBlock;
00084 typedef typename SubState::ConstCrossVarianceBlock ConstCrossVarianceBlock;
00085 typedef Block<const SystemMatrix,VectorDimension,SystemMatrix::ColsAtCompileTime> ConstSystemMatrixBlock;
00086 };
00087
00088 #define SYSTEM_MODEL_TRAIT(Derived, _VectorDimension, _CovarianceDimension) \
00089 typedef traits::SystemModel<Derived, _VectorDimension, _CovarianceDimension> trait; \
00090 \
00091 enum { VectorDimension = trait::VectorDimension }; \
00092 enum { CovarianceDimension = trait::CovarianceDimension }; \
00093 \
00094 typedef typename trait::StateVector StateVector; \
00095 typedef typename trait::NoiseVariance NoiseVariance; \
00096 typedef typename trait::SystemMatrix SystemMatrix; \
00097 \
00098 enum { InputDimension = trait::InputDimension }; \
00099 typedef typename trait::InputType InputType; \
00100 typedef typename trait::InputVector InputVector; \
00101 typedef typename trait::InputMatrix InputMatrix; \
00102 \
00103 typedef typename trait::SubState SubState; \
00104 typedef typename trait::Vector Vector; \
00105 \
00106 typedef typename trait::VectorSegment VectorSegment; \
00107 typedef typename trait::CovarianceBlock CovarianceBlock; \
00108 typedef typename trait::CrossVarianceBlock CrossVarianceBlock; \
00109 typedef typename trait::SystemMatrixBlock SystemMatrixBlock; \
00110 \
00111 typedef typename trait::ConstVectorSegment ConstVectorSegment; \
00112 typedef typename trait::ConstCovarianceBlock ConstCovarianceBlock; \
00113 typedef typename trait::ConstCrossVarianceBlock ConstCrossVarianceBlock; \
00114 typedef typename trait::ConstSystemMatrixBlock ConstSystemMatrixBlock; \
00115
00116 }
00117
00118 template <class Derived, int _VectorDimension = Dynamic, int _CovarianceDimension = _VectorDimension>
00119 class SystemModel_ : public SystemModel {
00120 public:
00121 SYSTEM_MODEL_TRAIT(Derived, _VectorDimension, _CovarianceDimension)
00122 virtual ~SystemModel_() {}
00123
00124 virtual SystemModel::SystemTypeEnum getSystemType() const { return SystemModel::TIME_DISCRETE; }
00125
00126 virtual void getPrior(State &state);
00127
00128 Derived *derived() { return static_cast<Derived *>(this); }
00129 const Derived *derived() const { return static_cast<const Derived *>(this); }
00130
00131 virtual void getExpectedDiff(StateVector& x_diff, const State& state, double dt);
00132 virtual void getStateJacobian(SystemMatrix& A, const State& state, double dt, bool init = true);
00133 virtual void getInputJacobian(InputMatrix& B, const State& state, double dt, bool init = true);
00134 virtual void getSystemNoise(NoiseVariance& Q, const State& state, double dt, bool init = true);
00135 };
00136
00137 template <class Derived, int _VectorDimension = Dynamic, int _CovarianceDimension = _VectorDimension>
00138 class TimeContinuousSystemModel_ : public SystemModel_<Derived, _VectorDimension, _CovarianceDimension> {
00139 public:
00140 SYSTEM_MODEL_TRAIT(Derived, _VectorDimension, _CovarianceDimension)
00141
00142 TimeContinuousSystemModel_();
00143 virtual ~TimeContinuousSystemModel_();
00144
00145 virtual SystemModel::SystemTypeEnum getSystemType() const { return SystemModel::TIME_CONTINUOUS; }
00146
00147 virtual void getDerivative(StateVector& x_dot, const State& state);
00148 virtual void getStateJacobian(SystemMatrix& A, const State& state, bool init = true);
00149 virtual void getInputJacobian(InputMatrix& B, const State& state, bool init = true);
00150 virtual void getSystemNoise(NoiseVariance& Q, const State& state, bool init = true);
00151
00152
00153 void getExpectedDiff(StateVector& x_diff, const State& state, double dt);
00154 void getStateJacobian(SystemMatrix& A, const State& state, double dt, bool init = true);
00155 void getInputJacobian(InputMatrix& B, const State& state, double dt, bool init = true);
00156 void getSystemNoise(NoiseVariance& Q, const State& state, double dt, bool init = true);
00157
00158 private:
00159 struct internal;
00160 struct internal *internal_;
00161 };
00162
00163 }
00164
00165 #include "system_model.inl"
00166
00167 #endif // HECTOR_POSE_ESTIMATION_SYSTEM_MODEL_H