예제 #1
0
  /** 
   * Sample this random vector and write the result over the mean x_;
   */
  void sample(){
    
    VecType x_sample, indep_noise;

    if(!isValid_Sx_L_){
      ::Eigen::LLT<MatType> cholesky( Sx_ );
      Sx_L_ = cholesky.matrixL();
      isValid_Sx_L_ = true;
    }

    if(gen_ == NULL){
      gen_ = new ::boost::variate_generator< ::boost::mt19937, 
					     ::boost::normal_distribution<double> >
	(::boost::mt19937(rand()), ::boost::normal_distribution<double>());
    }
    
    int n = Sx_L_.cols();
    for(int i = 0; i < n; i++){
      indep_noise(i) = (*gen_)();
    }
    x_ += Sx_L_ * indep_noise;

  }
예제 #2
0
파일: RandomVec.hpp 프로젝트: szma/RFS-SLAM
  /** 
   * Sample this random vector
   * \param[out] s_sample The sampled vector, with time and covariance copied from this random vector
   */
  void sample( RandomVec<VecType, MatType> &s_sample ){
    
    VecType x_sample, indep_noise;

    if(!isValid_Sx_L_){
      Eigen::LLT<MatType> cholesky( Sx_ );
      Sx_L_ = cholesky.matrixL();
      isValid_Sx_L_ = true;
    }

    if(gen_ == NULL){
      gen_ = new boost::variate_generator< boost::mt19937, 
					   boost::normal_distribution<double> >
	(boost::mt19937(rand()), boost::normal_distribution<double>());
    }
    
    int n = Sx_L_.cols();
    for(int i = 0; i < n; i++){
      indep_noise(i) = (*gen_)();
    }
    x_sample = x_ + Sx_L_ * indep_noise;
    s_sample.set( x_sample, Sx_, t_ );

  }