C++ MatType::get_row_count方法代码示例

typename boost::enable_if_c< is_continuous_belief_state<BeliefState>::value &&
                             (belief_state_traits<BeliefState>::representation == belief_representation::gaussian) &&
                             (belief_state_traits<BeliefState>::distribution == belief_distribution::unimodal),
void >::type unscented_kalman_predict(const System& sys,
                                      const StateSpaceType& state_space,
                                      BeliefState& b_x,
                                      const InputBelief& b_u,
                                      typename discrete_sss_traits<System>::time_type t = 0,
                                      typename belief_state_traits<BeliefState>::scalar_type alpha = 1E-3,
                                      typename belief_state_traits<BeliefState>::scalar_type kappa = 1,
                                      typename belief_state_traits<BeliefState>::scalar_type beta = 2) {
  //here the requirement is that the system models a linear system which is at worse a linearized system
  // - if the system is LTI or LTV, then this will result in a basic Kalman Filter (KF) prediction
  // - if the system is linearized, then this will result in an Extended Kalman Filter (EKF) prediction
  BOOST_CONCEPT_ASSERT((DiscreteSSSConcept< System, StateSpaceType >));
  BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<BeliefState>));
  BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<InputBelief>));
  
  using std::sqrt;
  
  typedef typename discrete_sss_traits<System>::point_type StateType;
  typedef typename continuous_belief_state_traits<BeliefState>::covariance_type CovType;
  typedef typename covariance_mat_traits< CovType >::matrix_type MatType;
  typedef typename mat_traits<MatType>::value_type ValueType;
  typedef typename vect_n<ValueType>::size_type SizeType;
  
  typedef typename continuous_belief_state_traits<InputBelief>::state_type InputType;
  typedef typename continuous_belief_state_traits<InputBelief>::covariance_type InputCovType;
  typedef typename covariance_mat_traits< InputCovType >::matrix_type InputMatType;
  
  StateType x = b_x.get_mean_state();
    
  MatType P = b_x.get_covariance().get_matrix();
  const InputMatType& Q = b_u.get_covariance().get_matrix();
  
  SizeType N = P.get_row_count();
  SizeType M = Q.get_row_count();
  
  mat<ValueType, mat_structure::square> L_p(N + M);
  mat<ValueType, mat_structure::square> P_aug(N + M);
  sub(P_aug)(range(0,N),range(0,N)) = P;
  sub(P_aug)(range(N,N+M),range(N,N+M)) = Q;
    
  try {
    decompose_Cholesky(P_aug,L_p);
  } catch(singularity_error&) {
    //use SVD instead.
    mat<ValueType, mat_structure::square> svd_U, svd_V;
    mat<ValueType, mat_structure::diagonal> svd_E;
    decompose_SVD(P_aug,svd_U,svd_E,svd_V);
    if(svd_E(0,0) < 0)
      throw singularity_error("'A-Priori Covariance P, in UKF prediction is singular, beyond repair!'");
    ValueType min_tolerable_sigma = sqrt(svd_E(0,0)) * 1E-2;
    for(unsigned int i = 0; i < svd_E.get_row_count(); ++i) {
      if(svd_E(i,i) < min_tolerable_sigma*min_tolerable_sigma)
        svd_E(i,i) = min_tolerable_sigma;
      else
        svd_E(i,i) = sqrt(svd_E(i,i));      
    };
    L_p = svd_U * svd_E;
    RK_WARNING("A-Priori Covariance P, in UKF prediction is singular, SVD was used, but this could hide a flaw in the system's setup.");
  };
  
  
  ValueType lambda = alpha * alpha * (N + M + kappa) - N - M;
  ValueType gamma = sqrt(ValueType(N + M) + lambda);
  
  vect_n< StateType > X_a(1 + 2 * (N + M));
  X_a[0] = sys.get_next_state(state_space, x, b_u.get_mean_state(), t);
  for(SizeType j = 0; j < N+M; ++j) {
    StateType x_right = x;
    StateType x_left = x;
    for(SizeType i = 0; i < N; ++i) {
      x_right[i] += gamma * L_p(i,j);
      x_left[i] -= gamma * L_p(i,j);
    };
    InputType u_right = b_u.get_mean_state();
    InputType u_left = b_u.get_mean_state();
    for(SizeType i = 0; i < M; ++i) {
      u_right[i] += gamma * L_p(N + i, j);
      u_left[i] -= gamma * L_p(N + i, j);
    };
    X_a[1+2*j] = sys.get_next_state(state_space, x_right, u_right, t);
    X_a[2+2*j] = sys.get_next_state(state_space, x_left, u_left, t);
  };
  
  gamma = ValueType(1) / (ValueType(N+M) + lambda);
  x = (lambda * gamma) * X_a[0];
  for(SizeType j = 0; j < N + M; ++j)
    x += (0.5 * gamma) * (X_a[1+2*j] + X_a[2+2*j]);
  for(SizeType j = 0; j < 1 + 2 * (N + M); ++j)
    X_a[j] -= x;
  
  ValueType W_c = (lambda * gamma + ValueType(1) - alpha * alpha + beta);
  for(SizeType i = 0; i < N; ++i)
    for(SizeType j = i; j < N; ++j)
      P(i,j) = W_c * X_a[0][i] * X_a[0][j];
  
  W_c = ValueType(0.5) * gamma;
  for(SizeType k = 1; k < 1 + 2 * (N + M); ++k)
//.........这里部分代码省略.........