00001 // $Id: conditionalUniforMeasPdf1d.cpp TDeLaet $ 00002 // Copyright (C) 2007 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 "conditionalUniformMeasPdf1d.h" 00020 #include <wrappers/rng/rng.h> // Wrapper around several rng libraries 00021 00022 #define MEASMODEL_NUMCONDARGUMENTS 1 00023 #define MEASMODEL_DIMENSION 1 00024 00025 namespace BFL 00026 { 00027 using namespace MatrixWrapper; 00028 00029 ConditionalUniformMeasPdf1d::ConditionalUniformMeasPdf1d(const Gaussian& measNoise) 00030 : ConditionalPdf<ColumnVector,int>(MEASMODEL_DIMENSION,MEASMODEL_NUMCONDARGUMENTS) 00031 { 00032 _measNoise = measNoise; 00033 } 00034 00035 00036 ConditionalUniformMeasPdf1d::~ConditionalUniformMeasPdf1d(){} 00037 00038 Probability 00039 ConditionalUniformMeasPdf1d::ProbabilityGet(const ColumnVector& measurement) const 00040 { 00041 // simplified version: the probability of a measurement is just the 00042 // probability under the additive Gaussian noise. The discrete nature of the 00043 // underlying state is not taken into account 00044 int state = ConditionalArgumentGet(0); 00045 ColumnVector expected_measurement(1); 00046 // the expected measurement in this simplified 1d example is just two times 00047 // the position of the 1d mobile robot 00048 expected_measurement(1) = 2 * state; 00049 00050 return _measNoise.ProbabilityGet(expected_measurement-measurement); 00051 } 00052 00053 }//namespace BFL 00054