Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "mobile_robot.h"
00020
00021 using namespace MatrixWrapper;
00022
00023 namespace BFL
00024 {
00025
00026 MobileRobot::MobileRobot():
00027 _state(STATE_SIZE)
00028 {
00029
00030 _state(1) = X_0;
00031 _state(2) = Y_0;
00032 _state(3) = THETA_0;
00033
00034
00035 ColumnVector sys_noise_Mu(STATE_SIZE);
00036 sys_noise_Mu(1) = MU_SYSTEM_NOISE_X_ROB;
00037 sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y_ROB;
00038 sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA_ROB;
00039 SymmetricMatrix sys_noise_Cov(STATE_SIZE);
00040 sys_noise_Cov = 0.0;
00041 sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X_ROB;
00042 sys_noise_Cov(1,2) = 0.0;
00043 sys_noise_Cov(1,3) = 0.0;
00044 sys_noise_Cov(2,1) = 0.0;
00045 sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y_ROB;
00046 sys_noise_Cov(2,3) = 0.0;
00047 sys_noise_Cov(3,1) = 0.0;
00048 sys_noise_Cov(3,2) = 0.0;
00049 sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA_ROB;
00050 _system_Uncertainty = new Gaussian(sys_noise_Mu, sys_noise_Cov);
00051
00052
00053 _sys_pdf = new NonLinearAnalyticConditionalGaussianMobile(*_system_Uncertainty);
00054 _sys_model = new AnalyticSystemModelGaussianUncertainty(_sys_pdf);
00055
00056
00057 SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
00058 meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE_ROB;
00059 ColumnVector meas_noise_Mu(MEAS_SIZE);
00060 meas_noise_Mu(1) = MU_MEAS_NOISE_ROB;
00061 _measurement_Uncertainty = new Gaussian(meas_noise_Mu, meas_noise_Cov);
00062
00063
00064 double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
00065 Matrix H(MEAS_SIZE,STATE_SIZE);
00066 H = 0.0;
00067 H(1,1) = wall_ct * RICO_WALL;
00068 H(1,2) = 0 - wall_ct;
00069 H(1,3) = 0.0;
00070
00071
00072 _meas_pdf = new LinearAnalyticConditionalGaussian(H, *_measurement_Uncertainty);
00073 _meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_meas_pdf);
00074
00075 }
00076
00077 MobileRobot::~MobileRobot()
00078 {
00079 delete _system_Uncertainty;
00080 delete _sys_pdf;
00081 delete _sys_model;
00082 delete _measurement_Uncertainty;
00083 delete _meas_pdf;
00084 delete _meas_model;
00085 }
00086
00087 void
00088 MobileRobot::Move(ColumnVector inputs)
00089 {
00090 _state = _sys_model->Simulate(_state,inputs);
00091 }
00092
00093
00094 ColumnVector
00095 MobileRobot::Measure()
00096 {
00097 return _meas_model->Simulate(_state);
00098 }
00099
00100
00101 ColumnVector
00102 MobileRobot::GetState()
00103 {
00104 return _state;
00105 }
00106 }
00107
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 Sun Oct 5 2014 22:29:53