UnscentedKalmanFilterTest.cpp
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  * @package ROBDEKON
17  * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
18  * @date 18.03.21
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  *
22  * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
23  * See also:
24  * - https://arxiv.org/pdf/2002.00878.pdf
25  * - https://ieeexplore.ieee.org/document/8206066/
26  */
27 #include "UnscentedKalmanFilter.h"
28 
29 #include <cstdlib> /* srand, rand */
30 #include <ctime> /* time */
31 
32 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
33 #include <ArmarXCore/core/logging/Logging.h>
34 #include <ArmarXCore/util/time.h>
35 
36 #include <sciplot/sciplot.hpp>
37 
38 using T = float;
39 using Vector = Eigen::Matrix<T, 3, 1>;
40 using Matrix = Eigen::Matrix<T, 3, 3>;
41 using SystemModelT = SystemModelSE3<T>;
42 
43 constexpr long num_timesteps = 3000;
44 constexpr T max_time = 1;
45 constexpr T dt = max_time / num_timesteps;
46 constexpr T c = (1 / max_time) * 2 * M_PI;
47 
48 constexpr T acc_noise_std = 0.01;
49 constexpr T om_noise_std = 0.01;
50 constexpr T obs_noise_std = 0.01;
51 constexpr T initial_offset_angle = 0.0 * 10 * M_PI / 180;
52 const Vector initial_offet_pos = 0.5 * Vector(1, 0.5, 0.7);
53 
54 void
55 test_retract()
56 {
57  for (int i = 0; i < num_timesteps; i++)
58  {
59  SystemModelT::StateT state1, state2;
60  state1.pose = manif::SE3<T>::Random();
61  // state1.velocity.setRandom();
62  state2.pose = manif::SE3<T>::Random();
63  // state2.velocity.setRandom();
64  // sigma = state2 - state1
65  SystemModelT::SigmaPointsT sigma = SystemModelT::inverseRetraction(state1, state2);
66  // state3 = state1 + sigma --> state3 = state2
67  SystemModelT::StateT state3 = SystemModelT::retraction(state1, sigma);
68  // ARMARX_CHECK((state2.velocity - state3.velocity).norm() < 10 * std::numeric_limits<T>::epsilon());
69  ARMARX_CHECK((state2.pose - state3.pose).coeffs().norm() <
70  10000 * std::numeric_limits<T>::epsilon())
71  << (state2.pose - state3.pose).coeffs().norm();
72 
73  SystemModelT::SigmaPointsT sigma2 = SystemModelT::inverseRetraction(state1, state3);
74  // TODO: fix bad accuracy
75  ARMARX_CHECK((sigma2 - sigma).norm() < 10000 * std::numeric_limits<T>::epsilon())
76  << (sigma2 - sigma).norm();
77  }
78 }
79 
80 void
81 test_se3()
82 {
83  for (int i = 1; i < num_timesteps; i++)
84  {
85  const T t = dt * i;
86  const T angle = t * c;
87  const Eigen::Matrix<T, 3, 1> pos(std::sin(angle), std::cos(angle), 0);
88  manif::SO3<T> rot = manif::SO3<T>(0, 0, angle);
89  ARMARX_CHECK(std::abs(rot.log().coeffs()(2) - angle) < 1e-6);
90  manif::SE3<T> pose(pos, rot);
91 
92  manif::SE3Tangent<T> tangent = pose.log();
93  ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 1e-6);
94  ARMARX_CHECK(std::abs(tangent.coeffs()(5) - angle) < 1e-6);
95  }
96 }
97 
98 void
99 simulate_trajectory(std::vector<SystemModelT::StateT>& states,
100  std::vector<SystemModelT::ControlT>& omegas)
101 {
102  SystemModelT::StateT state;
103  // state.velocity = Vector(c, 0, 0);
104  state.pose = manif::SE3<T>(0, 1, 0, M_PI, M_PI, 0);
105  states.push_back(state);
106  for (int i = 1; i < num_timesteps; i++)
107  {
108  const T t = dt * i;
109  const T t_prev = dt * (i - 1);
110  const T angle = t * c;
111  // yaw goes from -pi to pi
112  const T yaw = 1 * M_PI * std::sin(angle);
113  const T roll = 0.1 * M_PI * std::sin(angle) + M_PI;
114  const T pitch = 0.1 * M_PI * std::sin(angle) + M_PI;
115  const Vector pos(std::sin(angle), std::cos(angle), 0);
116  manif::SE3Tangent<T> tangent(states.at(i - 1).pose.log());
117  ARMARX_CHECK((states.at(i - 1).pose.translation() - pos).norm() < 0.1);
118  // ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 0.1);
119  const Vector vel(std::cos(angle) * c, -std::sin(angle) * c, 0);
120  const Vector acc(-std::sin(angle) * (c * c), -std::cos(angle) * (c * c), 0);
121  const Vector acc_prev(-std::sin(t_prev * c) * (c * c), -std::cos(t_prev * c) * (c * c), 0);
122  // const Eigen::Vector3f vel((pos - state.position) / dt);
123  // Eigen::Vector3f acc = (vel - state.velocity) / dt;
124  manif::SO3<T> rot = manif::SO3<T>(roll, pitch, yaw);
125  // Eigen::Matrix<T, 3, 1> rot_tan = rot.log().coeffs();
126  // ARMARX_CHECK(std::abs(rot_tan(2) - yaw) < 1e-6);
127  SystemModelT::ControlT control;
128  // control.linear_accel = state.pose.rotation().inverse() * acc_prev;
129  Vector angular_velocity(0.1 * M_PI * std::cos(angle) * c,
130  0.1 * M_PI * std::cos(angle) * c,
131  M_PI * std::cos(angle) * c);
132  control.velocity.coeffs() << state.pose.asSO3().inverse().act(vel), angular_velocity;
133 
134  const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
135  state, control, SystemModelT::ControlNoiseT::Zero(), dt);
136  manif::SE3Tangent<T> pro_tangent = propagated.pose.log();
137  T pos_diff1 = (propagated.pose.translation() - pos).norm();
138  // ARMARX_CHECK(pos_diff1 < pos_diff2);
139  ARMARX_CHECK(pos_diff1 < 2e-5)
140  << "Position is not the same: " << pos << " vs " << propagated.pose.translation();
141  // ARMARX_CHECK((propagated.velocity - vel).norm() < 1e-4) << "Velocity is not the same: " << vel << " vs " << propagated.velocity;
142  // ARMARX_CHECK(propagated.pose.asSO3().lminus(rot).coeffs().norm() < 1e-2) << "Rotation is not the same: " << rot.rotation() << " vs " << propagated.pose.rotation();
143  // state.velocity = vel;
144  state.pose = manif::SE3<T>(pos, rot);
145  states.push_back(propagated);
146 
147  tangent = state.pose.log();
148  ARMARX_CHECK((state.pose.translation() - pos).norm() < 1e-6);
149  // ARMARX_CHECK(std::abs(tangent.coeffs()(5) - yaw) < 1e-2);
150 
151  // add noise
152  control.velocity.coeffs().segment(0, 3) += acc_noise_std * Vector::Random();
153  control.velocity.coeffs().segment(3, 3) += om_noise_std * Vector::Random();
154  omegas.push_back(control);
155  }
156 }
157 
158 void
159 simulate_observation(const std::vector<SystemModelT::StateT>& states,
160  std::vector<SystemModelT::ObsT>& observations)
161 {
162  for (const auto& state : states)
163  {
164  SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
165  SystemModelT::ObsT true_obs;
166  true_obs = state.pose.log().coeffs();
167  // true_obs.segment(0, 3) = state.pose.translation();
168  // true_obs.segment(3, 3) = state.pose.asSO3().log().coeffs();
169  ARMARX_CHECK((obs - true_obs).norm() < std::numeric_limits<T>::epsilon());
170  observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
171  }
172 }
173 
174 int
175 main()
176 {
177  srand(time(NULL));
178 
179  test_retract();
180  // test_se3();
181 
182  std::vector<SystemModelT::StateT> states;
183  std::vector<SystemModelT::ControlT> omegas;
184  std::vector<SystemModelT::ObsT> observations;
185  simulate_trajectory(states, omegas);
186  simulate_observation(states, observations);
187 
188  ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
189  << " Num Obs: " << observations.size();
190  UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
191  UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
192  Q.block<3, 3>(0, 0) *= acc_noise_std * acc_noise_std;
193  Q.block<3, 3>(3, 3) *= om_noise_std * om_noise_std;
194 
195  UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
196  UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std * obs_noise_std;
197  UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
198  UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
199  P0.block<3, 3>(3, 3) *= initial_offset_angle * initial_offset_angle;
200  P0.block<3, 3>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
201  // P0.block<3, 3>(6, 6) *= 0.0;
202  UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
203  UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
204 
205  SystemModelT::StateT state0;
206  manif::SO3<T> rot(0, 0, initial_offset_angle);
207  state0.pose = states.at(0).pose.lplus(manif::SE3<T>(initial_offet_pos, rot).log());
208  // state0.velocity = states.at(0).velocity;
209  UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
210 
211  std::vector<SystemModelT::StateT> ukf_states;
212  std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
213  ukf_states.push_back(state0);
214  ukf_Ps.push_back(P0);
215 
216  std::vector<T> x_true, y_true, z_true, x_obs, y_obs, z_obs, x_ukf, y_ukf, z_ukf;
217  std::vector<T> vx_true, vy_true, vz_true, vx_ukf, vy_ukf, vz_ukf;
218  std::vector<T> a_true, b_true, c_true, a_obs, b_obs, c_obs, a_ukf, b_ukf, c_ukf;
219 
220  for (int i = 1; i < num_timesteps; i++)
221  {
222  // propagate
223  TIMING_START(LOOP);
224  TIMING_START(PROPAGATION);
225  ukf.propagation(omegas.at(i - 1), dt);
226  TIMING_END(PROPAGATION);
227  if ((i - 1) % 100 == 0)
228  {
229  TIMING_START(UPDATE);
230  ukf.update(observations.at(i));
231  TIMING_END(UPDATE);
232  TIMING_START(REST);
233  const SystemModelT::StateT& current_state = ukf.getCurrentState();
234  // ARMARX_INFO << "Velocity Diff: " << (states.at(i).velocity - current_state.velocity).norm();
235  ARMARX_INFO << "Pose Diff: "
236  << (states.at(i).pose - current_state.pose).coeffs().norm();
237  // ARMARX_INFO << "Vel: " << current_state.velocity - states.at(i).velocity;
238  ARMARX_INFO << "Max Cov " << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff();
239  ARMARX_INFO
240  << "Diag: "
241  << (ukf.getCurrentStateCovariance() - Eigen::Matrix<T, 6, 6>::Identity()).norm();
242  // ARMARX_CHECK((states.at(i).position - current_state.position).norm() < 1);
243 
244  x_true.push_back(states.at(i).pose.log().coeffs()(0));
245  y_true.push_back(states.at(i).pose.log().coeffs()(1));
246  z_true.push_back(states.at(i).pose.log().coeffs()(2));
247 
248  x_obs.push_back(observations.at(i)(0));
249  y_obs.push_back(observations.at(i)(1));
250  z_obs.push_back(observations.at(i)(2));
251 
252  x_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(0));
253  y_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(1));
254  z_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(2));
255 
256  // vx_true.push_back(states.at(i).velocity(0, 0));
257  // vy_true.push_back(states.at(i).velocity(1, 0));
258  // vz_true.push_back(states.at(i).velocity(2, 0));
259  //
260  // vx_ukf.push_back(ukf.getCurrentState().velocity(0, 0));
261  // vy_ukf.push_back(ukf.getCurrentState().velocity(1, 0));
262  // vz_ukf.push_back(ukf.getCurrentState().velocity(2, 0));
263 
264  a_true.push_back(states.at(i).pose.log().coeffs()(3));
265  b_true.push_back(states.at(i).pose.log().coeffs()(4));
266  c_true.push_back(states.at(i).pose.log().coeffs()(5));
267 
268  a_obs.push_back(observations.at(i)(3, 0));
269  b_obs.push_back(observations.at(i)(4, 0));
270  c_obs.push_back(observations.at(i)(5, 0));
271 
272  a_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(3));
273  b_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(4));
274  c_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(5));
275  TIMING_END(REST);
276  }
277 
278  ukf_states.push_back(ukf.getCurrentState());
279  ukf_Ps.push_back(ukf.getCurrentStateCovariance());
280 
281  TIMING_END(LOOP);
282  }
283 
284  sciplot::Plot3D position_plot;
285  sciplot::Plot pos_plot;
286 
287  position_plot.drawCurve(x_true, y_true, z_true).label("True").lineWidth(1);
288  position_plot.drawCurve(x_obs, y_obs, z_obs).label("Obs").lineWidth(1);
289  position_plot.drawCurve(x_ukf, y_ukf, z_ukf).label("UKF").lineWidth(1);
290 
291  pos_plot.drawCurve(x_true, y_true).label("True");
292  pos_plot.drawCurve(x_obs, y_obs).label("Obs");
293  pos_plot.drawCurve(x_ukf, y_ukf).label("UKF");
294 
295  // sciplot::Plot3D vel_plot;
296  //
297  // vel_plot.drawCurve(vx_true, vy_true, vz_true).label("True").lineWidth(1);
298  // vel_plot.drawCurve(vx_ukf, vy_ukf, vz_ukf).label("UKF").lineWidth(1);
299 
300 
301  sciplot::Plot3D orientation_plot;
302 
303  orientation_plot.drawCurve(a_true, b_true, c_true).label("True").lineWidth(1);
304  orientation_plot.drawCurve(a_obs, b_obs, c_obs).label("Obs").lineWidth(1);
305  orientation_plot.drawCurve(a_ukf, b_ukf, c_ukf).label("UKF").lineWidth(1);
306 
307  pos_plot.show();
308  position_plot.show();
309  // vel_plot.show();
310  orientation_plot.show();
311 }
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:289
time.h
UnscentedKalmanFilter
Definition: UnscentedKalmanFilter.h:36
TIMING_END
#define TIMING_END(name)
Definition: TimeUtil.h:306
max_time
constexpr T max_time
Definition: UnscentedKalmanFilterTest.cpp:44
Vector
Eigen::Matrix< T, 3, 1 > Vector
Definition: UnscentedKalmanFilterTest.cpp:39
acc_noise_std
constexpr T acc_noise_std
Definition: UnscentedKalmanFilterTest.cpp:48
UnscentedKalmanFilter::propagation
void propagation(const ControlT &omega, FloatT dt)
Definition: UnscentedKalmanFilter.cpp:53
initial_offset_angle
constexpr T initial_offset_angle
Definition: UnscentedKalmanFilterTest.cpp:51
simulate_trajectory
void simulate_trajectory(std::vector< SystemModelT::StateT > &states, std::vector< SystemModelT::ControlT > &omegas)
Definition: UnscentedKalmanFilterTest.cpp:99
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
ControlSE3
Definition: SystemModel.h:45
SystemModelSE3::retraction
static StateT retraction(const StateT &state, const SigmaPointsT &sigmaPoints)
Definition: SystemModel.cpp:63
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
SystemModelSE3::inverseRetraction
static SigmaPointsT inverseRetraction(const StateT &state1, const StateT &state2)
Definition: SystemModel.cpp:54
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
M_PI
#define M_PI
Definition: MathTools.h:17
StateSE3
Definition: SystemModel.h:39
initial_offet_pos
const Vector initial_offet_pos
Definition: UnscentedKalmanFilterTest.cpp:52
SystemModelSE3::observationFunction
static ObsT observationFunction(const StateT &state)
Definition: SystemModel.cpp:34
obs_noise_std
constexpr T obs_noise_std
Definition: UnscentedKalmanFilterTest.cpp:50
UnscentedKalmanFilter::update
void update(const ObsT &y)
Definition: UnscentedKalmanFilter.cpp:121
test_se3
void test_se3()
Definition: UnscentedKalmanFilterTest.cpp:81
SystemModelSE3::propagationFunction
static StateT propagationFunction(const StateT &state, const ControlT &control, const ControlNoiseT &noise, FloatT dt)
Definition: SystemModel.cpp:42
ExpressionException.h
simulate_observation
void simulate_observation(const std::vector< SystemModelT::StateT > &states, std::vector< SystemModelT::ObsT > &observations)
Definition: UnscentedKalmanFilterTest.cpp:159
UnscentedKalmanFilter::getCurrentState
const StateT & getCurrentState() const
Definition: UnscentedKalmanFilter.cpp:170
om_noise_std
constexpr T om_noise_std
Definition: UnscentedKalmanFilterTest.cpp:49
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
num_timesteps
constexpr long num_timesteps
Definition: UnscentedKalmanFilterTest.cpp:43
main
int main()
Definition: UnscentedKalmanFilterTest.cpp:175
UnscentedKalmanFilter::getCurrentStateCovariance
const StateCovT & getCurrentStateCovariance() const
Definition: UnscentedKalmanFilter.cpp:177
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:109
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
UnscentedKalmanFilter.h
SystemModelSE3
Definition: SystemModel.h:78
Logging.h
T
float T
Definition: UnscentedKalmanFilterTest.cpp:38
test_retract
void test_retract()
Definition: UnscentedKalmanFilterTest.cpp:55
control
This file is part of ArmarX.
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:45
StateSE3::pose
manif::SE3< floatT > pose
Definition: SystemModel.h:41
norm
double norm(const Point &a)
Definition: point.hpp:102