mobile_robot.cpp
Go to the documentation of this file.
00001 // $Id: mobile_robot.cpp tdelaet $
00002 // Copyright (C) 2006  Tinne De Laet <first dot last at mech dot kuleuven dot be>
00003 //
00004 // This program is free software; you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published by
00006 // the Free Software Foundation; either version 2.1 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
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         // initial state
00030         _state(1) = X_0;
00031         _state(2) = Y_0;
00032         _state(3) = THETA_0;
00033 
00034         // sys noise
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     // create the model
00053     _sys_pdf = new NonLinearAnalyticConditionalGaussianMobile(*_system_Uncertainty);
00054     _sys_model = new AnalyticSystemModelGaussianUncertainty(_sys_pdf);
00055 
00056         // meas noise
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     // create matrix _meas_model for linear measurement model
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     // create the measurement model
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