HumanSystemModel.h
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
17  * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
18  * @date 2022
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 #pragma once
23 
24 #include <manif/SE2.h>
25 #include <manif/SO2.h>
26 
28 {
29  //----------- SO2xR2 -----------------
30 
31  template <typename floatT>
32  struct StateSO2xR2
33  {
34  manif::SO2<floatT> orientation;
36  };
37 
38  template <typename floatT>
40  {
41  typename manif::SO2<floatT>::Tangent angular_velocity;
43  };
44 
45  template <typename floatT>
47  {
48  static_assert(std::is_floating_point_v<floatT>);
49  struct dim
50  {
51  static constexpr long state = 3, control = 3, obs = 3;
52  };
53 
54  using FloatT = floatT;
58  using ControlNoiseT = Eigen::
59  Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
61 
62  static ObsT observationFunction(const StateT& state);
63 
64  static StateT propagationFunction(const StateT& state,
65  const ControlT& control,
66  const ControlNoiseT& noise,
67  FloatT dt);
68 
69  static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
70 
71  static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
72  };
73 
74  extern template struct SystemModelSO2xR2<float>;
75  extern template struct SystemModelSO2xR2<double>;
76 
77 
78  //----------- SE2 -----------------
79 
80  template <typename floatT>
81  struct StateSE2
82  {
83  manif::SE2<floatT> pose;
84  };
85 
86  template <typename floatT>
87  struct ControlSE2
88  {
89  typename manif::SE2<floatT>::Tangent velocity;
90  };
91 
92  template <typename floatT>
94  {
95  static_assert(std::is_floating_point_v<floatT>);
96  struct dim
97  {
98  static constexpr long state = 3, control = 3, obs = 3;
99  };
100 
101  using FloatT = floatT;
105  using ControlNoiseT = Eigen::
106  Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
108 
109  static ObsT observationFunction(const StateT& state);
110 
111  static StateT propagationFunction(const StateT& state,
112  const ControlT& control,
113  const ControlNoiseT& noise,
114  FloatT dt);
115 
116  static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
117 
118  static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
119  };
120 
121  extern template struct SystemModelSE2<float>;
122  extern template struct SystemModelSE2<double>;
123 
124 
125  //----------- SE2xV -----------------
126 
127  template <typename floatT>
128  struct StateSE2xV
129  {
130  manif::SE2<floatT> pose;
131  typename manif::SE2<floatT>::Tangent velocity;
132  };
133 
134  template <typename floatT>
136  {
137  };
138 
139  template <typename floatT>
141  {
142  static_assert(std::is_floating_point_v<floatT>);
143  struct dim
144  {
145  static constexpr long state = 6, control = 1, obs = 3;
146  };
147 
148  using FloatT = floatT;
152  using ControlNoiseT = Eigen::
153  Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
155 
156  static ObsT observationFunction(const StateT& state);
157 
158  static StateT propagationFunction(const StateT& state,
159  const ControlT& control,
160  const ControlNoiseT& noise,
161  FloatT dt);
162 
163  static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
164 
165  static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
166  };
167 
168  extern template struct SystemModelSE2xV<float>;
169  extern template struct SystemModelSE2xV<double>;
170 
171 } // namespace armarx::navigation::human::kalman_filter
armarx::navigation::human::kalman_filter::StateSO2xR2
Definition: HumanSystemModel.h:32
armarx::navigation::human::kalman_filter::SystemModelSE2
Definition: HumanSystemModel.h:93
armarx::navigation::human::kalman_filter::SystemModelSE2xV::dim
Definition: HumanSystemModel.h:143
armarx::navigation::human::kalman_filter::SystemModelSE2xV
Definition: HumanSystemModel.h:140
armarx::navigation::human::kalman_filter::StateSE2xV::pose
manif::SE2< floatT > pose
Definition: HumanSystemModel.h:130
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::observationFunction
static ObsT observationFunction(const StateT &state)
Definition: HumanSystemModel.cpp:10
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::dim
Definition: HumanSystemModel.h:49
armarx::navigation::human::kalman_filter::SystemModelSO2xR2
Definition: HumanSystemModel.h:46
armarx::navigation::human::kalman_filter::StateSE2xV::velocity
manif::SE2< floatT >::Tangent velocity
Definition: HumanSystemModel.h:131
armarx::navigation::human::kalman_filter::ControlSO2xR2::euclidean_velocity
Eigen::Matrix< floatT, 2, 1 > euclidean_velocity
Definition: HumanSystemModel.h:42
armarx::navigation::human::kalman_filter::SystemModelSE2::FloatT
floatT FloatT
Definition: HumanSystemModel.h:101
armarx::navigation::human::kalman_filter::StateSO2xR2::position
Eigen::Matrix< floatT, 2, 1 > position
Definition: HumanSystemModel.h:35
armarx::navigation::human::kalman_filter::StateSO2xR2::orientation
manif::SO2< floatT > orientation
Definition: HumanSystemModel.h:34
armarx::navigation::human::kalman_filter::StateSE2xV
Definition: HumanSystemModel.h:128
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::propagationFunction
static StateT propagationFunction(const StateT &state, const ControlT &control, const ControlNoiseT &noise, FloatT dt)
Definition: HumanSystemModel.cpp:20
armarx::navigation::human::kalman_filter::StateSE2
Definition: HumanSystemModel.h:81
armarx::navigation::human::kalman_filter::SystemModelSE2xV::FloatT
floatT FloatT
Definition: HumanSystemModel.h:148
armarx::navigation::human::kalman_filter::ControlSE2::velocity
manif::SE2< floatT >::Tangent velocity
Definition: HumanSystemModel.h:89
armarx::navigation::human::kalman_filter::ControlSE2xV
Definition: HumanSystemModel.h:135
armarx::navigation::human::kalman_filter::SystemModelSE2::dim
Definition: HumanSystemModel.h:96
armarx::navigation::human::kalman_filter::ControlSO2xR2
Definition: HumanSystemModel.h:39
armarx::navigation::human::kalman_filter::ControlSO2xR2::angular_velocity
manif::SO2< floatT >::Tangent angular_velocity
Definition: HumanSystemModel.h:41
armarx::navigation::human::kalman_filter::ControlSE2
Definition: HumanSystemModel.h:87
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::inverseRetraction
static SigmaPointsT inverseRetraction(const StateT &state1, const StateT &state2)
Definition: HumanSystemModel.cpp:50
armarx::navigation::human::kalman_filter::StateSE2::pose
manif::SE2< floatT > pose
Definition: HumanSystemModel.h:83
Eigen::Matrix< floatT, 2, 1 >
armarx::navigation::human::kalman_filter
This file is part of ArmarX.
Definition: HumanSystemModel.cpp:3
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::dim::obs
static constexpr long obs
Definition: HumanSystemModel.h:51
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::dim::state
static constexpr long state
Definition: HumanSystemModel.h:51
control
This file is part of ArmarX.
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::FloatT
floatT FloatT
Definition: HumanSystemModel.h:54
armarx::navigation::human::kalman_filter::SystemModelSO2xR2::retraction
static StateT retraction(const StateT &state, const SigmaPointsT &sigmaPoints)
Definition: HumanSystemModel.cpp:35
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:42