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