00001 // $Id: nonlinearanalyticconditionalgaussianmobile.cpp 5823 2005-10-27 13:43:02Z 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 "nonlinearMeasurementPdf.h" 00020 #include <wrappers/rng/rng.h> // Wrapper around several rng libraries 00021 00022 #define MEASMODEL_NUMCONDARGUMENTS_MOBILE 1 00023 #define MEASMODEL_DIMENSION_MOBILE 3 00024 00025 namespace BFL 00026 { 00027 using namespace MatrixWrapper; 00028 00029 NonlinearMeasurementPdf::NonlinearMeasurementPdf(const Gaussian& measNoise) 00030 : ConditionalPdf<ColumnVector,ColumnVector>(MEASMODEL_DIMENSION_MOBILE,MEASMODEL_NUMCONDARGUMENTS_MOBILE) 00031 { 00032 _measNoise = measNoise; 00033 } 00034 00035 00036 NonlinearMeasurementPdf::~NonlinearMeasurementPdf(){} 00037 00038 Probability 00039 NonlinearMeasurementPdf::ProbabilityGet(const ColumnVector& measurement) const 00040 { 00041 ColumnVector state = ConditionalArgumentGet(0); 00042 00043 ColumnVector expected_measurement(1); 00044 expected_measurement(1) = 2 * state(2); 00045 00046 return _measNoise.ProbabilityGet(expected_measurement-measurement); 00047 } 00048 00049 }//namespace BFL 00050