61 params_ = paramfactory->getOdometryModel();
100 double current_time = x_true.
getTime();
109 adoreMatrix<double,7,1>
x;
119 x = dlib::colm(X,X.nc()-1);
121 const double fade_out =
std::min(1.0,std::abs(x_true.
getvx()));
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