ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
odometrymodel.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2021 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/sim/afactory.h>
17 #include <adore/params/afactory.h>
18 #include <adore/mad/oderk4.h>
19 #include <adore/mad/adoremath.h>
21 #include <iostream>
22 #include <random>
23 
24 namespace adore
25 {
26 
27  namespace apps
28  {
34  {
35  private:
36  double last_time_;
41  std::default_random_engine generator_;
42  std::normal_distribution<double> distribution_;
43 
50  public:
53  unsigned int seed = 0): generator_(seed),distribution_(0.0,1.0)
54  {
55  std::srand(seed);
56  relative_to_map_ = true;
57  integration_step_ = 0.005;
58  reset_pose_feed_ = sim_factory->getVehiclePoseResetFeed();
59  vehicle_model_input_ = sim_factory->getVehicleMotionStateReader();
60  odometry_estimate_output_ = sim_factory->getOdometryEstimatedVehicleStateWriter();
61  params_ = paramfactory->getOdometryModel();
62  }
63 
64  double nrand()
65  {
66  return distribution_(generator_);
67  }
68 
69 
74  virtual void update()
75  {
76  if( reset_pose_feed_->hasNext() )
77  {
81  {
82  x_estimate_.setX(0.0);
83  x_estimate_.setY(0.0);
84  x_estimate_.setZ(0.0);
85  x_estimate_.setPSI(0.0);
86  }
87  else
88  {
89  x_estimate_.setX(pose.getX());
90  x_estimate_.setY(pose.getY());
91  x_estimate_.setZ(pose.getY());
92  x_estimate_.setPSI(pose.getPSI());
93  }
94  }
95 
97  {
100  double current_time = x_true.getTime();
101  last_time_ = adore::mad::bound(current_time-1.0,last_time_,current_time);
102 
103 
104  if(last_time_valid_ )
105  {
106  if( current_time>last_time_ )
107  {
108  auto T = adore::mad::sequence(last_time_,integration_step_,current_time);
109  adoreMatrix<double,7,1> x;
110  x(0) = x_estimate_.getX();
111  x(1) = x_estimate_.getY();
112  x(2) = x_estimate_.getPSI();
113  x(3) = x_estimate_.getvx();
114  x(4) = x_estimate_.getvy();
115  x(5) = x_estimate_.getOmega();
116  x(6) = x_estimate_.getAx();
118  auto X = solver_.solve(&model,T,x);
119  x = dlib::colm(X,X.nc()-1);
120 
121  const double fade_out = std::min(1.0,std::abs(x_true.getvx()));//do not drift while in standstill
122  const double e_vx = fade_out * params_->get_k_e_vx() * nrand();
123  const double e_vy = fade_out * params_->get_k_e_vy() * nrand();
124  const double e_omega = fade_out * params_->get_k_e_omega() * nrand();
125  const double e_ax = fade_out * params_->get_k_e_ax() * nrand();
126 
127 
128  x_estimate_.setX(x(0));
129  x_estimate_.setY(x(1));
130  x_estimate_.setPSI(x(2));
131  x_estimate_.setvx(x_true.getvx()+e_vx);
132  x_estimate_.setvy(x_true.getvy()+e_vy);
133  x_estimate_.setOmega(x_true.getOmega()+e_omega);
134  x_estimate_.setAx(x_true.getAx()+e_ax);
135  x_estimate_.setDelta(x_true.getDelta());
136  x_estimate_.setTime(current_time);
137  last_time_ = current_time;
138  }
140  }
141  else
142  {
143  x_estimate_.setvx(x_true.getvx());
144  x_estimate_.setvy(x_true.getvy());
145  x_estimate_.setOmega(x_true.getOmega());
146  x_estimate_.setAx(x_true.getAx());
147  x_estimate_.setDelta(x_true.getDelta());
148  x_estimate_.setTime(current_time);
149  last_time_ = current_time;
150  last_time_valid_ = true;
151  }
152  }
153  }
154 
155  };
156  }
157 }
a model for odometry sensor integrates velocities as measured with errors
Definition: odometrymodel.h:34
adore::mad::AWriter< adore::fun::VehicleMotionState9d > * odometry_estimate_output_
Definition: odometrymodel.h:45
virtual void update()
simulation step of the odometry estimate model
Definition: odometrymodel.h:74
double integration_step_
Definition: odometrymodel.h:37
double last_time_
Definition: odometrymodel.h:36
adore::mad::AReader< adore::fun::VehicleMotionState9d > * vehicle_model_input_
Definition: odometrymodel.h:44
double nrand()
Definition: odometrymodel.h:64
adore::mad::AFeed< adore::sim::ResetVehiclePose > * reset_pose_feed_
Definition: odometrymodel.h:47
bool relative_to_map_
Definition: odometrymodel.h:40
std::normal_distribution< double > distribution_
Definition: odometrymodel.h:42
std::default_random_engine generator_
Definition: odometrymodel.h:41
adore::params::APOdometryModel * params_
Definition: odometrymodel.h:48
adore::fun::VehicleMotionState9d x_estimate_
Definition: odometrymodel.h:39
adore::mad::OdeRK4< double > solver_
Definition: odometrymodel.h:46
OdometryModel(adore::sim::AFactory *sim_factory=adore::sim::SimFactoryInstance::get(), adore::params::AFactory *paramfactory=adore::params::ParamsFactoryInstance::get(), unsigned int seed=0)
Definition: odometrymodel.h:51
bool last_time_valid_
Definition: odometrymodel.h:38
kinematic vehicle model
Definition: kinematic_model.h:29
virtual bool hasNext() const =0
virtual void getLatest(T &value)=0
virtual void getData(T &value)=0
virtual bool hasData() const =0
virtual void write(const T &value)=0
virtual adoreMatrix< T > solve(AOdeModel< T > *model, const adoreMatrix< double, 1, 0 > &time, const adoreMatrix< double, 0, 1 > &x0) override
Definition: oderk4.h:37
abstract factory for adore::params classes
Definition: afactory.h:54
abstract class containing parameters which configure odometry state estimation model
Definition: ap_odometrymodel.h:26
virtual double get_k_e_vy() const =0
virtual double get_k_e_ax() const =0
virtual double get_k_e_vx() const =0
virtual double get_k_e_omega() const =0
static adore::params::AFactory * get()
Definition: afactory.h:103
abstract factory for adore::sim communication
Definition: afactory.h:43
static adore::sim::AFactory * get()
Definition: afactory.h:145
adoreMatrix< T, 1, 0 > sequence(T x0, T dx, T xend)
Definition: adoremath.h:51
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
x
Definition: adore_set_goal.py:30
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
void setZ(double value)
Set the z-coordinate.
Definition: vehiclemotionstate9d.h:127
void setY(double value)
Set the y-coordinate.
Definition: vehiclemotionstate9d.h:121
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getvx() const
Get the longitudinal velocity.
Definition: vehiclemotionstate9d.h:78
void setAx(double value)
Set the longitudinal acceleration.
Definition: vehiclemotionstate9d.h:157
double getOmega() const
Get the yaw rate.
Definition: vehiclemotionstate9d.h:90
void setvx(double value)
set the longitudinal velocity
Definition: vehiclemotionstate9d.h:139
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72
double getAx() const
Get the longitudinal acceleration.
Definition: vehiclemotionstate9d.h:96
void setOmega(double value)
Set the yaw rate.
Definition: vehiclemotionstate9d.h:151
double getvy() const
Get the lateral velocity.
Definition: vehiclemotionstate9d.h:84
void setX(double value)
Set the x-coordinate.
Definition: vehiclemotionstate9d.h:115
double getDelta() const
Get the steering angle.
Definition: vehiclemotionstate9d.h:102
void setPSI(double value)
set the heading
Definition: vehiclemotionstate9d.h:133
void setvy(double value)
set the lateral velocity
Definition: vehiclemotionstate9d.h:145
void setDelta(double value)
Set the steering angle.
Definition: vehiclemotionstate9d.h:163
void setTime(double value)
Set the time.
Definition: vehiclemotionstate9d.h:109
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48
provides encapsulation of values needed to reset the vehicle pose in a simulation
Definition: resetvehiclepose.h:26
double getX() const
Definition: resetvehiclepose.h:35
double getY() const
Definition: resetvehiclepose.h:43
double getPSI() const
Definition: resetvehiclepose.h:51