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 "nonlinearSystemPdf.h" 00020 #include <wrappers/rng/rng.h> // Wrapper around several rng libraries 00021 00022 #define SYSMODEL_NUMCONDARGUMENTS_MOBILE 2 00023 #define SYSMODEL_DIMENSION_MOBILE 3 00024 00025 namespace BFL 00026 { 00027 using namespace MatrixWrapper; 00028 00029 NonlinearSystemPdf::NonlinearSystemPdf(const Gaussian& additiveNoise) 00030 : ConditionalPdf<ColumnVector,ColumnVector>(SYSMODEL_DIMENSION_MOBILE,SYSMODEL_NUMCONDARGUMENTS_MOBILE) 00031 { 00032 _additiveNoise = additiveNoise; 00033 } 00034 00035 00036 NonlinearSystemPdf::~NonlinearSystemPdf(){} 00037 00038 00039 bool NonlinearSystemPdf::SampleFrom (Sample<ColumnVector>& one_sample, int method, void * args) const 00040 { 00041 ColumnVector state = ConditionalArgumentGet(0); 00042 ColumnVector vel = ConditionalArgumentGet(1); 00043 00044 // system update 00045 state(1) += cos(state(3)) * vel(1); 00046 state(2) += sin(state(3)) * vel(1); 00047 state(3) += vel(2); 00048 00049 // sample from additive noise 00050 Sample<ColumnVector> noise; 00051 _additiveNoise.SampleFrom(noise, method, args); 00052 00053 // store results in one_sample 00054 one_sample.ValueSet(state + noise.ValueGet()); 00055 00056 return true; 00057 } 00058 00059 }//namespace BFL 00060