00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef transform_gaussian_H
00029 #define transform_gaussian_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/math/math_frwds.h>
00033 #include <mrpt/math/CMatrixTemplateNumeric.h>
00034 #include <mrpt/math/CMatrixFixedNumeric.h>
00035 #include <mrpt/math/ops_matrices.h>
00036 #include <mrpt/math/jacobians.h>
00037 #include <mrpt/random.h>
00038
00039 namespace mrpt
00040 {
00041 namespace math
00042 {
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
00056 void transform_gaussian_unscented(
00057 const VECTORLIKE1 &x_mean,
00058 const MATLIKE1 &x_cov,
00059 void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param, VECTORLIKE3 &y),
00060 const USERPARAM &fixed_param,
00061 VECTORLIKE2 &y_mean,
00062 MATLIKE2 &y_cov,
00063 const bool *elem_do_wrap2pi = NULL,
00064 const double alpha = 1e-3,
00065 const double K = 0,
00066 const double beta = 2.0
00067 )
00068 {
00069 MRPT_START
00070 const size_t Nx = x_mean.size();
00071 const double lambda = alpha*alpha*(Nx+K)-Nx;
00072 const double c = Nx+lambda;
00073
00074
00075 const double Wi = 0.5/c;
00076 vector_double W_mean(1+2*Nx,Wi),W_cov(1+2*Nx,Wi);
00077 W_mean[0] = lambda/c;
00078 W_cov[0] = W_mean[0]+(1-alpha*alpha+beta);
00079
00080
00081 MATLIKE1 L;
00082 const bool valid = x_cov.chol(L);
00083 if (!valid) throw std::runtime_error("transform_gaussian_unscented: Singular covariance matrix in Cholesky.");
00084 L*= sqrt(c);
00085
00086
00087
00088 typename mrpt::aligned_containers<VECTORLIKE3>::vector_t Y(1+2*Nx);
00089 VECTORLIKE1 X = x_mean;
00090 functor(X,fixed_param,Y[0]);
00091 VECTORLIKE1 delta;
00092 delta.resize(Nx);
00093 size_t row=1;
00094 for (size_t i=0;i<Nx;i++)
00095 {
00096 L.extractRowAsCol(i,delta);
00097 X=x_mean;X-=delta;
00098 functor(X,fixed_param,Y[row++]);
00099 X=x_mean;X+=delta;
00100 functor(X,fixed_param,Y[row++]);
00101 }
00102
00103
00104 mrpt::math::covariancesAndMeanWeighted(Y, y_cov,y_mean,&W_mean,&W_cov,elem_do_wrap2pi);
00105 MRPT_END
00106 }
00107
00108
00109
00110
00111
00112
00113
00114 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
00115 void transform_gaussian_montecarlo(
00116 const VECTORLIKE1 &x_mean,
00117 const MATLIKE1 &x_cov,
00118 void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
00119 const USERPARAM &fixed_param,
00120 VECTORLIKE2 &y_mean,
00121 MATLIKE2 &y_cov,
00122 const size_t num_samples = 1000,
00123 typename mrpt::aligned_containers<VECTORLIKE3>::vector_t *out_samples_y = NULL
00124 )
00125 {
00126 MRPT_START
00127 typename mrpt::aligned_containers<VECTORLIKE1>::vector_t samples_x;
00128 mrpt::random::randomGenerator.drawGaussianMultivariateMany(samples_x,num_samples,x_cov,&x_mean);
00129 typename mrpt::aligned_containers<VECTORLIKE3>::vector_t samples_y(num_samples);
00130 for (size_t i=0;i<num_samples;i++)
00131 functor(samples_x[i],fixed_param,samples_y[i]);
00132 mrpt::math::covariancesAndMean(samples_y,y_cov,y_mean);
00133 if (out_samples_y) { out_samples_y->clear(); samples_y.swap(*out_samples_y); }
00134 MRPT_END
00135 }
00136
00137
00138
00139
00140
00141
00142
00143 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
00144 void transform_gaussian_linear(
00145 const VECTORLIKE1 &x_mean,
00146 const MATLIKE1 &x_cov,
00147 void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
00148 const USERPARAM &fixed_param,
00149 VECTORLIKE2 &y_mean,
00150 MATLIKE2 &y_cov,
00151 const VECTORLIKE1 &x_increments
00152 )
00153 {
00154 MRPT_START
00155
00156 functor(x_mean,fixed_param,y_mean);
00157
00158 Eigen::Matrix<double,VECTORLIKE3::RowsAtCompileTime,VECTORLIKE1::RowsAtCompileTime> H;
00159 mrpt::math::jacobians::jacob_numeric_estimate(x_mean,functor,x_increments,fixed_param,H);
00160 H.multiply_HCHt(x_cov, y_cov);
00161 MRPT_END
00162 }
00163
00164
00165
00166 }
00167
00168 }
00169
00170
00171 #endif