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
00030 #ifndef MCL_3DL_MOTION_PREDICTION_MODELS_MOTION_PREDICTION_MODEL_DIFFERENTIAL_DRIVE_H
00031 #define MCL_3DL_MOTION_PREDICTION_MODELS_MOTION_PREDICTION_MODEL_DIFFERENTIAL_DRIVE_H
00032
00033 #include <mcl_3dl/motion_prediction_model_base.h>
00034
00035 namespace mcl_3dl
00036 {
00037 class MotionPredictionModelDifferentialDrive : public MotionPredictionModelBase
00038 {
00039 public:
00040 MotionPredictionModelDifferentialDrive(const float odom_err_integ_lin_tc, const float odom_err_integ_ang_tc)
00041 : odom_err_integ_lin_tc_(odom_err_integ_lin_tc)
00042 , odom_err_integ_ang_tc_(odom_err_integ_ang_tc)
00043 {
00044 }
00045
00046 inline void setOdoms(const State6DOF& odom_prev, const State6DOF& odom_current, const float time_diff) final
00047 {
00048 relative_translation_ = odom_prev.rot_.inv() * (odom_current.pos_ - odom_prev.pos_);
00049 relative_quat_ = odom_prev.rot_.inv() * odom_current.rot_;
00050 Vec3 axis;
00051 relative_quat_.getAxisAng(axis, relative_angle_);
00052 relative_translation_norm_ = relative_translation_.norm();
00053 time_diff_ = time_diff;
00054 };
00055
00056 inline void predict(State6DOF& s) const final
00057 {
00058 const Vec3 diff = relative_translation_ * (1.0 + s.noise_ll_) + Vec3(s.noise_al_ * relative_angle_, 0.0, 0.0);
00059 s.odom_err_integ_lin_ += (diff - relative_translation_);
00060 s.pos_ += s.rot_ * diff;
00061 const float yaw_diff = s.noise_la_ * relative_translation_norm_ + s.noise_aa_ * relative_angle_;
00062 s.rot_ = Quat(Vec3(0.0, 0.0, 1.0), yaw_diff) * s.rot_ * relative_quat_;
00063 s.rot_.normalize();
00064 s.odom_err_integ_ang_ += Vec3(0.0, 0.0, yaw_diff);
00065 s.odom_err_integ_lin_ *= (1.0 - time_diff_ / odom_err_integ_lin_tc_);
00066 s.odom_err_integ_ang_ *= (1.0 - time_diff_ / odom_err_integ_ang_tc_);
00067 };
00068
00069 private:
00070 const float odom_err_integ_lin_tc_;
00071 const float odom_err_integ_ang_tc_;
00072 Vec3 relative_translation_;
00073 Quat relative_quat_;
00074 float relative_translation_norm_;
00075 float relative_angle_;
00076 float time_diff_;
00077 };
00078 }
00079
00080 #endif // MCL_3DL_MOTION_PREDICTION_MODELS_MOTION_PREDICTION_MODEL_DIFFERENTIAL_DRIVE_H