compare_filters/nonlinearanalyticconditionalgaussianmobile.cpp
Go to the documentation of this file.
1 // \$Id: nonlinearanalyticconditionalgaussianmobile.cpp 5823 2005-10-27 13:43:02Z TDeLaet \$
2 // Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
3 //
4 // This program is free software; you can redistribute it and/or modify
6 // the Free Software Foundation; either version 2.1 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 //
18
20 #include <wrappers/rng/rng.h> // Wrapper around several rng
21  // libraries
22 #define NUMCONDARGUMENTS_MOBILE 2
23
24 namespace BFL
25 {
26  using namespace MatrixWrapper;
27
28
31  {
32  }
33
34
36
38  {
39  ColumnVector state = ConditionalArgumentGet(0);
40  ColumnVector vel = ConditionalArgumentGet(1);
41  state(1) += cos(state(3)) * vel(1);
42  state(2) += sin(state(3)) * vel(1);
43  state(3) += vel(2);
45  }
46
48  {
50  {
51  if (i==0)//derivative to the first conditional argument (x)
52  {
53  ColumnVector state = ConditionalArgumentGet(0);
54  ColumnVector vel = ConditionalArgumentGet(1);
55  Matrix df(3,3);
56  df(1,1)=1;
57  df(1,2)=0;
58  df(1,3)=-vel(1)*sin(state(3));
59  df(2,1)=0;
60  df(2,2)=1;
61  df(2,3)=vel(1)*cos(state(3));
62  df(3,1)=0;
63  df(3,2)=0;
64  df(3,3)=1;
65  return df;
66  }
67  else
68  {
69  cerr << "The df is not implemented for the" <<i << "th conditional argument\n";
70  exit(-BFL_ERRMISUSE);
71  }
72  }
73  else
74  {
75  cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n";
76  exit(-BFL_ERRMISUSE);
77  }
78
79  }
80 }//namespace BFL
81
Constructor.
virtual ~NonLinearAnalyticConditionalGaussianMobile()
Destructor.
virtual MatrixWrapper::ColumnVector ExpectedValueGet() const
Get the expected value E[x] of the pdf.
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
Get the mean Value of the Additive Gaussian uncertainty.
Abstract Class representing all full Analytical Conditional gaussians with Additive Gaussian Noise...
#define NUMCONDARGUMENTS_MOBILE
virtual MatrixWrapper::Matrix dfGet(unsigned int i) const
returns derivative from function to n-th conditional variable
#define BFL_ERRMISUSE
unsigned int NumConditionalArgumentsGet() const
Get the Number of conditional arguments.
const MatrixWrapper::ColumnVector & ConditionalArgumentGet(unsigned int n_argument) const
Get the n-th argument of the list.

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 Mon Jun 10 2019 12:47:59