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)
for(SizeType i = 0; i < N; ++i)
for(SizeType j = i; j < N; ++j)
P(i,j) += W_c * X_a[k][i] * X_a[k][j];
b_x.set_mean_state(x);
b_x.set_covariance( CovType( P ) );
};
