ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
localizationmodel.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 <iostream>
19 #include <random>
20 
21 namespace adore
22 {
23 
24  namespace apps
25  {
31  {
32  private:
33  std::default_random_engine generator_;
34  std::normal_distribution<double> ndistribution_;
35  std::uniform_real_distribution<double> udistribution_;
36  double eXdrift;
37  double eYdrift;
38  double eXjump;
39  double eYjump;
40  double ePSI;
42  double last_time_;
43 
48  public:
51  unsigned int seed = 0): generator_(seed),ndistribution_(0.0,1.0),udistribution_(-1.0,1.0)
52  {
53  std::srand(seed);
54  vehicle_model_input_ = sim_factory->getVehicleMotionStateReader();
55  localization_estimate_output_ = sim_factory->getLocalizationEstimatedVehicleStateWriter();
56  params_ = paramfactory->getLocalizationModel();
57  eXjump = 0.0;
58  eYjump = 0.0;
59  eXdrift = 0.0;
60  eYdrift = 0.0;
61  ePSI = 0.0;
62  last_time_valid_ = false;
63  }
64 
65  double nrand()
66  {
67  return ndistribution_(generator_);
68  }
69 
70  double rand()
71  {
72  return udistribution_(generator_);
73  }
74 
75 
80  virtual void update()
81  {
83  {
87  x_estimate = x_true;
88  if(!last_time_valid_)
89  {
90  //initialize with an offset
91  double rjump = nrand() * params_->get_jump_deviation_pos();
92  double phijump = rand() * 3.141592653589793;
93  eXjump = std::cos(phijump)*rjump - std::sin(phijump)*rjump;
94  eYjump = std::sin(phijump)*rjump + std::cos(phijump)*rjump;
96  last_time_valid_ = true;
97  }
98  else
99  {
100  if(last_time_==x_true.getTime())return;
101  }
102  last_time_ = x_true.getTime();
103 
104  double rdrift = nrand() * params_->get_drift_deviation_pos();
105  double phidrift = rand() * 3.141592653589793;
106  double eXtarget = std::cos(phidrift)*rdrift - std::sin(phidrift)*rdrift;
107  double eYtarget = std::sin(phidrift)*rdrift + std::cos(phidrift)*rdrift;
110  eXdrift+=dX;
111  eYdrift+=dY;
112  if(rand()*0.5+0.5<params_->get_jump_threshold_pos())
113  {
114  double rjump = nrand() * params_->get_jump_deviation_pos();
115  double phijump = rand() * 3.141592653589793;
116  eXjump = std::cos(phijump)*rjump - std::sin(phijump)*rjump;
117  eYjump = std::sin(phijump)*rjump + std::cos(phijump)*rjump;
118  }
119  if(rand()*0.5+0.5<params_->get_jump_threshold_heading())
120  {
122  }
123 
124 
125  x_estimate.setX(x_true.getX() + eXjump + eXdrift);
126  x_estimate.setY(x_true.getY() + eYjump + eYdrift);
127  x_estimate.setPSI(x_true.getPSI() + ePSI);
129  }
130  }
131 
132  };
133  }
134 }
a model for localization adds errors to true vehicle state
Definition: localizationmodel.h:31
adore::params::APLocalizationModel * params_
Definition: localizationmodel.h:46
adore::mad::AReader< adore::fun::VehicleMotionState9d > * vehicle_model_input_
Definition: localizationmodel.h:44
std::normal_distribution< double > ndistribution_
Definition: localizationmodel.h:34
adore::mad::AWriter< adore::fun::VehicleMotionState9d > * localization_estimate_output_
Definition: localizationmodel.h:45
double nrand()
Definition: localizationmodel.h:65
virtual void update()
simulation step of the odometry estimate model
Definition: localizationmodel.h:80
LocalizationModel(adore::sim::AFactory *sim_factory=adore::sim::SimFactoryInstance::get(), adore::params::AFactory *paramfactory=adore::params::ParamsFactoryInstance::get(), unsigned int seed=0)
Definition: localizationmodel.h:49
double eXjump
Definition: localizationmodel.h:38
std::uniform_real_distribution< double > udistribution_
Definition: localizationmodel.h:35
double eYdrift
Definition: localizationmodel.h:37
double ePSI
Definition: localizationmodel.h:40
double eXdrift
Definition: localizationmodel.h:36
double eYjump
Definition: localizationmodel.h:39
double last_time_
Definition: localizationmodel.h:42
double rand()
Definition: localizationmodel.h:70
std::default_random_engine generator_
Definition: localizationmodel.h:33
bool last_time_valid_
Definition: localizationmodel.h:41
virtual void getData(T &value)=0
virtual bool hasData() const =0
virtual void write(const T &value)=0
abstract factory for adore::params classes
Definition: afactory.h:54
abstract class containing parameters which configure localization state estimation model
Definition: ap_localizationmodel.h:26
virtual double get_jump_threshold_pos() const =0
virtual double get_jump_deviation_pos() const =0
virtual double get_drift_deviation_pos() const =0
virtual double get_jump_threshold_heading() const =0
virtual double get_drift_rate_pos() const =0
virtual double get_jump_deviation_heading() 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
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
void setY(double value)
Set the y-coordinate.
Definition: vehiclemotionstate9d.h:121
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72
void setX(double value)
Set the x-coordinate.
Definition: vehiclemotionstate9d.h:115
void setPSI(double value)
set the heading
Definition: vehiclemotionstate9d.h:133
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48