mobile_robot.cpp
Go to the documentation of this file.
1 // $Id: mobile_robot.cpp tdelaet $
2 // Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published by
6 // the Free Software Foundation; either version 2.1 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 //
18 
19 #include "mobile_robot.h"
20 
21 using namespace MatrixWrapper;
22 
23 namespace BFL
24 {
25 
26  MobileRobot::MobileRobot():
27  _state(STATE_SIZE)
28  {
29  // initial state
30  _state(1) = X_0;
31  _state(2) = Y_0;
32  _state(3) = THETA_0;
33 
34  // sys noise
35  ColumnVector sys_noise_Mu(STATE_SIZE);
36  sys_noise_Mu(1) = MU_SYSTEM_NOISE_X_ROB;
37  sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y_ROB;
38  sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA_ROB;
39  SymmetricMatrix sys_noise_Cov(STATE_SIZE);
40  sys_noise_Cov = 0.0;
41  sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X_ROB;
42  sys_noise_Cov(1,2) = 0.0;
43  sys_noise_Cov(1,3) = 0.0;
44  sys_noise_Cov(2,1) = 0.0;
45  sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y_ROB;
46  sys_noise_Cov(2,3) = 0.0;
47  sys_noise_Cov(3,1) = 0.0;
48  sys_noise_Cov(3,2) = 0.0;
49  sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA_ROB;
50  _system_Uncertainty = new Gaussian(sys_noise_Mu, sys_noise_Cov);
51 
52  // create the model
55 
56  // meas noise
57  SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
58  meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE_ROB;
59  ColumnVector meas_noise_Mu(MEAS_SIZE);
60  meas_noise_Mu(1) = MU_MEAS_NOISE_ROB;
61  _measurement_Uncertainty = new Gaussian(meas_noise_Mu, meas_noise_Cov);
62 
63  // create matrix _meas_model for linear measurement model
64  double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
65  Matrix H(MEAS_SIZE,STATE_SIZE);
66  H = 0.0;
67  H(1,1) = wall_ct * RICO_WALL;
68  H(1,2) = 0 - wall_ct;
69  H(1,3) = 0.0;
70 
71  // create the measurement model
74 
75  }
76 
78  {
79  delete _system_Uncertainty;
80  delete _sys_pdf;
81  delete _sys_model;
83  delete _meas_pdf;
84  delete _meas_model;
85  }
86 
87  void
88  MobileRobot::Move(ColumnVector inputs)
89  {
90  _state = _sys_model->Simulate(_state,inputs);
91  }
92 
93 
94  ColumnVector
96  {
97  return _meas_model->Simulate(_state);
98  }
99 
100 
101  ColumnVector
103  {
104  return _state;
105  }
106 }
107 
MatrixWrapper::ColumnVector GetState()
MatrixWrapper::ColumnVector _state
Definition: mobile_robot.h:65
LinearAnalyticMeasurementModelGaussianUncertainty * _meas_model
Definition: mobile_robot.h:64
MatrixWrapper::ColumnVector Measure()
#define X_0
#define MEAS_SIZE
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
#define MU_SYSTEM_NOISE_Y_ROB
#define MU_SYSTEM_NOISE_X_ROB
#define RICO_WALL
#define STATE_SIZE
#define SIGMA_SYSTEM_NOISE_Y_ROB
#define THETA_0
Gaussian * _measurement_Uncertainty
Definition: mobile_robot.h:62
MeasVar Simulate(const StateVar &x, const StateVar &s, int sampling_method=DEFAULT, void *sampling_args=NULL)
Simulate the Measurement, given a certain state, and an input.
void Move(MatrixWrapper::ColumnVector inputs)
#define SIGMA_MEAS_NOISE_ROB
#define SIGMA_SYSTEM_NOISE_X_ROB
Class for analytic system models with additive Gauss. uncertainty.
Class for linear analytic measurementmodels with additive gaussian noise.
#define MU_MEAS_NOISE_ROB
T Simulate(const T &x, const T &u, int sampling_method=DEFAULT, void *sampling_args=NULL)
Simulate the system.
Definition: systemmodel.h:126
Gaussian * _system_Uncertainty
Definition: mobile_robot.h:59
AnalyticSystemModelGaussianUncertainty * _sys_model
Definition: mobile_robot.h:61
LinearAnalyticConditionalGaussian * _meas_pdf
Definition: mobile_robot.h:63
#define SIGMA_SYSTEM_NOISE_THETA_ROB
#define Y_0
#define MU_SYSTEM_NOISE_THETA_ROB
NonLinearAnalyticConditionalGaussianMobile * _sys_pdf
Definition: mobile_robot.h:60


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 28 2022 21:56:33