motion_prediction_model_differential_drive.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef MCL_3DL_MOTION_PREDICTION_MODELS_MOTION_PREDICTION_MODEL_DIFFERENTIAL_DRIVE_H
31 #define MCL_3DL_MOTION_PREDICTION_MODELS_MOTION_PREDICTION_MODEL_DIFFERENTIAL_DRIVE_H
32 
34 
35 namespace mcl_3dl
36 {
38 {
39 public:
40  MotionPredictionModelDifferentialDrive(const float odom_err_integ_lin_tc, const float odom_err_integ_ang_tc)
41  : odom_err_integ_lin_tc_(odom_err_integ_lin_tc)
42  , odom_err_integ_ang_tc_(odom_err_integ_ang_tc)
43  {
44  }
45 
46  inline void setOdoms(const State6DOF& odom_prev, const State6DOF& odom_current, const float time_diff) final
47  {
48  relative_translation_ = odom_prev.rot_.inv() * (odom_current.pos_ - odom_prev.pos_);
49  relative_quat_ = odom_prev.rot_.inv() * odom_current.rot_;
50  Vec3 axis;
53  time_diff_ = time_diff;
54  };
55 
56  inline void predict(State6DOF& s) const final
57  {
58  const Vec3 diff = relative_translation_ * (1.0 + s.noise_ll_) + Vec3(s.noise_al_ * relative_angle_, 0.0, 0.0);
59  s.odom_err_integ_lin_ += (diff - relative_translation_);
60  s.pos_ += s.rot_ * diff;
61  const float yaw_diff = s.noise_la_ * relative_translation_norm_ + s.noise_aa_ * relative_angle_;
62  s.rot_ = Quat(Vec3(0.0, 0.0, 1.0), yaw_diff) * s.rot_ * relative_quat_;
63  s.rot_.normalize();
64  s.odom_err_integ_ang_ += Vec3(0.0, 0.0, yaw_diff);
65  s.odom_err_integ_lin_ *= (1.0 - time_diff_ / odom_err_integ_lin_tc_);
66  s.odom_err_integ_ang_ *= (1.0 - time_diff_ / odom_err_integ_ang_tc_);
67  };
68 
69 private:
70  const float odom_err_integ_lin_tc_;
76  float time_diff_;
77 };
78 } // namespace mcl_3dl
79 
80 #endif // MCL_3DL_MOTION_PREDICTION_MODELS_MOTION_PREDICTION_MODEL_DIFFERENTIAL_DRIVE_H
XmlRpcServer s
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
float norm() const
Definition: vec3.h:153
MotionPredictionModelDifferentialDrive(const float odom_err_integ_lin_tc, const float odom_err_integ_ang_tc)
constexpr Quat inv() const
Definition: quat.h:187
void getAxisAng(Vec3 &axis, float &ang) const
Definition: quat.h:226
void setOdoms(const State6DOF &odom_prev, const State6DOF &odom_current, const float time_diff) final


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29