Go to the documentation of this file.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_IMU_MODEL_H
00030 #define HECTOR_POSE_ESTIMATION_IMU_MODEL_H
00031
00032 #include <hector_pose_estimation/system_model.h>
00033 #include <hector_pose_estimation/system.h>
00034
00035 namespace hector_pose_estimation {
00036
00037 class GyroModel : public TimeContinuousSystemModel_<GyroModel, 3>
00038 {
00039 public:
00040 enum StateIndex {
00041 BIAS_GYRO_X = 0,
00042 BIAS_GYRO_Y,
00043 BIAS_GYRO_Z
00044 };
00045
00046 GyroModel();
00047 virtual ~GyroModel();
00048
00049 SubState& sub(State& state) const { return *drift_; }
00050 const SubState& sub(const State& state) const { return *drift_; }
00051
00052 virtual bool init(PoseEstimation& estimator, State& state);
00053
00054 using TimeContinuousSystemModel_<GyroModel, 3>::getSystemNoise;
00055 virtual void getSystemNoise(NoiseVariance& Q, const State& state, bool init);
00056 using TimeContinuousSystemModel_<GyroModel, 3>::getStateJacobian;
00057 virtual void getStateJacobian(SystemMatrix& A1, CrossSystemMatrix& A01, const State& state, bool init);
00058
00059 ConstStateVectorSegment getBias() const { return drift_->getVector(); }
00060
00061 private:
00062 SubStatePtr drift_;
00063 double rate_stddev_;
00064 double rate_drift_;
00065 };
00066
00067 class AccelerometerModel : public TimeContinuousSystemModel_<AccelerometerModel, 3>
00068 {
00069 public:
00070 enum StateIndex {
00071 BIAS_ACCEL_X = 0,
00072 BIAS_ACCEL_Y,
00073 BIAS_ACCEL_Z
00074 };
00075
00076 AccelerometerModel();
00077 virtual ~AccelerometerModel();
00078
00079 SubState& sub(State& state) const { return *drift_; }
00080 const SubState& sub(const State& state) const { return *drift_; }
00081
00082 virtual bool init(PoseEstimation& estimator, State& state);
00083
00084 using TimeContinuousSystemModel_<AccelerometerModel, 3>::getSystemNoise;
00085 virtual void getSystemNoise(NoiseVariance& Q, const State& state, bool init);
00086 using TimeContinuousSystemModel_<AccelerometerModel, 3>::getStateJacobian;
00087 virtual void getStateJacobian(SystemMatrix& A1, CrossSystemMatrix& A01, const State& state, bool init);
00088
00089 ConstStateVectorSegment getBias() const { return drift_->getVector(); }
00090
00091 private:
00092 SubStatePtr drift_;
00093 double acceleration_stddev_;
00094 double acceleration_drift_;
00095 };
00096
00097 typedef System_<GyroModel> Gyro;
00098 extern template class System_<GyroModel>;
00099
00100 typedef System_<AccelerometerModel> Accelerometer;
00101 extern template class System_<AccelerometerModel>;
00102
00103 }
00104
00105 #endif // HECTOR_POSE_ESTIMATION_IMU_MODEL_H