PlatformKalmanFilter.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarX
19  * @author Mirko Waechter( mirko.waechter at kit dot edu)
20  * @date 2018
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 #include "KalmanFilter.h"
27 
28 #include <IceUtil/Time.h>
29 #include <memory>
30 
31 namespace memoryx
32 {
33  /**
34  * @brief This class is a convenience class for a holonomic platform using a Kalman filter.
35  */
37  {
38  public:
39  /**
40  * @brief PlatformKalmanFilter
41  * @param initialPosition Initial position in mm
42  * @param initialRotation Initial rotation in rad
43  * @param translationSensorStdDev Standard deviation of the position sensing in mm
44  * @param rotationSensorStdDev Standard deviation of the orientation sensing in rad
45  * @param translationVelocitySensorStdDev Standard deviation of the translation velocity sensing in mm. This value is scaled with the current velocity in the predict step.
46  * @param rotationVelocitySensorStdDev Standard deviation of the rotation velocity sensing in rad. This value is scaled with the current velocity in the predict step.
47  */
48  PlatformKalmanFilter(Eigen::Vector2d initialPosition, double initialRotation, double translationSensorStdDev = 100.0, double rotationSensorStdDev = 0.1,
49  double translationVelocitySensorStdDev = 1., double rotationVelocitySensorStdDev = 1.);
50  /**
51  * @brief Performs the predict-step of the Kalman filter.
52  * @param velX translational velocity on x-axis of the platform in **robot coordinates**
53  * @param velY translational velocity on y-axis of the platform in **robot coordinates**
54  * @param velTheta rotational velocity on z-axis of the platform in **robot coordinates**
55  * @param deltaT timestep between last and current velocity measurement, i.e. between last and current predict() call
56  */
57  void predict(double velX, double velY, double velTheta, const IceUtil::Time& deltaT);
58 
59  /**
60  * @brief Performs the update-step of the Kalman filter.
61  * @param x X-position of the platform in **global coordinates**
62  * @param y Y-position of the platform in **global coordinates**
63  * @param alpha Rotation of the platform in **global coordinates**
64  */
65  void update(double x, double y, double alpha);
66 
67  /**
68  * @brief Global pose of the holonomic platform.
69  * @return x,y,rotation
70  */
71  const Eigen::Vector3d& getPose() const;
72 
73  Eigen::Vector2d getPosition() const;
74  double getOrientation() const;
75 
76  /**
77  * @brief Covariance matrix of the current belief of the state
78  */
79  Eigen::Matrix3d getCovariance() const;
80 
81  const KalmanFilter& getFilter() const;
82 
83  /**
84  * @brief Change the internal Kalman Filter. Only use if you know what you are doing.
85  */
86  void setFilter(const KalmanFilter& value);
87 
88  protected:
89  Eigen::Vector4d transformPose(const Eigen::Vector3d& pose) const;
90  Eigen::Vector3d inverseTransformPose(const Eigen::Vector4d& pose) const;
91 
92 
96  Eigen::Vector3d pose;
97  };
98  using PlatformKalmanFilterPtr = std::shared_ptr<PlatformKalmanFilter>;
99 
100 }
101 
memoryx::PlatformKalmanFilter::state
Gaussian state
Definition: PlatformKalmanFilter.h:94
memoryx::PlatformKalmanFilter::transformPose
Eigen::Vector4d transformPose(const Eigen::Vector3d &pose) const
Definition: PlatformKalmanFilter.cpp:128
KalmanFilter
Definition: KalmanFilter.h:27
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
memoryx::PlatformKalmanFilter::filter
KalmanFilter filter
Definition: PlatformKalmanFilter.h:93
memoryx::PlatformKalmanFilter::getOrientation
double getOrientation() const
Definition: PlatformKalmanFilter.cpp:104
memoryx::PlatformKalmanFilter::getCovariance
Eigen::Matrix3d getCovariance() const
Covariance matrix of the current belief of the state.
Definition: PlatformKalmanFilter.cpp:109
memoryx::PlatformKalmanFilter
This class is a convenience class for a holonomic platform using a Kalman filter.
Definition: PlatformKalmanFilter.h:36
memoryx::PlatformKalmanFilter::getPosition
Eigen::Vector2d getPosition() const
Definition: PlatformKalmanFilter.cpp:99
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
memoryx::PlatformKalmanFilter::update
void update(double x, double y, double alpha)
Performs the update-step of the Kalman filter.
Definition: PlatformKalmanFilter.cpp:84
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
memoryx::PlatformKalmanFilter::getPose
const Eigen::Vector3d & getPose() const
Global pose of the holonomic platform.
Definition: PlatformKalmanFilter.cpp:94
KalmanFilter.h
memoryx::PlatformKalmanFilterPtr
std::shared_ptr< PlatformKalmanFilter > PlatformKalmanFilterPtr
Definition: PlatformKalmanFilter.h:98
memoryx::PlatformKalmanFilter::PlatformKalmanFilter
PlatformKalmanFilter(Eigen::Vector2d initialPosition, double initialRotation, double translationSensorStdDev=100.0, double rotationSensorStdDev=0.1, double translationVelocitySensorStdDev=1., double rotationVelocitySensorStdDev=1.)
PlatformKalmanFilter.
Definition: PlatformKalmanFilter.cpp:29
memoryx::PlatformKalmanFilter::setFilter
void setFilter(const KalmanFilter &value)
Change the internal Kalman Filter.
Definition: PlatformKalmanFilter.cpp:122
memoryx::PlatformKalmanFilter::pose
Eigen::Vector3d pose
Definition: PlatformKalmanFilter.h:96
Gaussian
Definition: Gaussian.h:46
memoryx::PlatformKalmanFilter::motionNoise
Gaussian motionNoise
Definition: PlatformKalmanFilter.h:95
memoryx::PlatformKalmanFilter::predict
void predict(double velX, double velY, double velTheta, const IceUtil::Time &deltaT)
Performs the predict-step of the Kalman filter.
Definition: PlatformKalmanFilter.cpp:58
memoryx::PlatformKalmanFilter::getFilter
const KalmanFilter & getFilter() const
Definition: PlatformKalmanFilter.cpp:117
memoryx::PlatformKalmanFilter::inverseTransformPose
Eigen::Vector3d inverseTransformPose(const Eigen::Vector4d &pose) const
Definition: PlatformKalmanFilter.cpp:137