nonlinearanalyticconditionalgaussianodo.cpp
Go to the documentation of this file.
1 // Copyright (C) 2008 Wim Meeussen <meeussen at willowgarage com>
2 //
3 // This program is free software; you can redistribute it and/or modify
4 // it under the terms of the GNU Lesser General Public License as published by
5 // the Free Software Foundation; either version 2.1 of the License, or
6 // (at your option) any later version.
7 //
8 // This program is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 // GNU Lesser General Public License for more details.
12 //
13 // You should have received a copy of the GNU Lesser General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
16 //
17 
19 #include <bfl/wrappers/rng/rng.h> // Wrapper around several rng libraries
20 #define NUMCONDARGUMENTS_MOBILE 2
21 
22 namespace BFL
23 {
24  using namespace MatrixWrapper;
25 
26 
28  : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE),
29  df(6,6)
30  {
31  // initialize df matrix
32  for (unsigned int i=1; i<=6; i++){
33  for (unsigned int j=1; j<=6; j++){
34  if (i==j) df(i,j) = 1;
35  else df(i,j) = 0;
36  }
37  }
38  }
39 
40 
42 
44  {
45  ColumnVector state = ConditionalArgumentGet(0);
46  ColumnVector vel = ConditionalArgumentGet(1);
47  state(1) += cos(state(6)) * vel(1);
48  state(2) += sin(state(6)) * vel(1);
49  state(6) += vel(2);
50  return state + AdditiveNoiseMuGet();
51  }
52 
54  {
55  if (i==0)//derivative to the first conditional argument (x)
56  {
57  double vel_trans = ConditionalArgumentGet(1)(1);
58  double yaw = ConditionalArgumentGet(0)(6);
59 
60  df(1,3)=-vel_trans*sin(yaw);
61  df(2,3)= vel_trans*cos(yaw);
62 
63  return df;
64  }
65  else
66  {
67  if (i >= NumConditionalArgumentsGet())
68  {
69  cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n";
70  exit(-BFL_ERRMISUSE);
71  }
72  else{
73  cerr << "The df is not implemented for the" <<i << "th conditional argument\n";
74  exit(-BFL_ERRMISUSE);
75  }
76  }
77  }
78 
79 }//namespace BFL
80 
virtual MatrixWrapper::Matrix dfGet(unsigned int i) const
virtual MatrixWrapper::ColumnVector ExpectedValueGet() const
NonLinearAnalyticConditionalGaussianOdo(const Gaussian &additiveNoise)
Constructor.


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:12