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 #include <hector_pose_estimation/system/imu_input.h>
00036
00037 namespace hector_pose_estimation {
00038
00039 class GyroModel : public TimeContinuousSystemModel_<GyroModel,3>
00040 {
00041 public:
00042 using TimeContinuousSystemModel_<GyroModel,3>::SubState;
00043
00044 GyroModel();
00045 virtual ~GyroModel();
00046
00047 bool init(PoseEstimation& estimator, System &system, State& state);
00048 void getPrior(State &state);
00049
00050 ColumnVector3 getError() const { return bias_->getVector(); }
00051
00052 ColumnVector3 getRate(const ImuInput::RateType& imu_rate, const State& state) const;
00053 void getRateJacobian(SystemMatrixBlock& C, const State& state, bool init = true);
00054 void getRateNoise(CovarianceBlock Q, const State& state, bool init = true);
00055
00056 using TimeContinuousSystemModel_<GyroModel,3>::getSystemNoise;
00057 void getSystemNoise(NoiseVariance& Q, const State& state, bool init = true);
00058
00059 private:
00060 typename SubState::Ptr bias_;
00061 double rate_stddev_;
00062 double rate_drift_;
00063 };
00064
00065 class AccelerometerModel : public TimeContinuousSystemModel_<AccelerometerModel,3>
00066 {
00067 public:
00068 using TimeContinuousSystemModel_<AccelerometerModel,3>::SubState;
00069
00070 AccelerometerModel();
00071 virtual ~AccelerometerModel();
00072
00073 bool init(PoseEstimation& estimator, System &system, State& state);
00074 void getPrior(State &state);
00075
00076 ColumnVector3 getError() const { return bias_->getVector(); }
00077
00078 ColumnVector3 getAcceleration(const ImuInput::AccelerationType& imu_acceleration, const State& state) const;
00079 void getAccelerationJacobian(SystemMatrixBlock& C, const State& state, bool init = true);
00080 void getAccelerationNoise(CovarianceBlock Q, const State& state, bool init = true);
00081
00082 using TimeContinuousSystemModel_<AccelerometerModel,3>::getSystemNoise;
00083 void getSystemNoise(NoiseVariance& Q, const State& state, bool init = true);
00084
00085 private:
00086 typename SubState::Ptr bias_;
00087 double acceleration_stddev_;
00088 double acceleration_drift_;
00089 };
00090
00091 typedef System_<GyroModel> Gyro;
00092 extern template class System_<GyroModel>;
00093
00094 typedef System_<AccelerometerModel> Accelerometer;
00095 extern template class System_<AccelerometerModel>;
00096
00097 }
00098
00099 #endif // HECTOR_POSE_ESTIMATION_IMU_MODEL_H