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 invariant_kalman_update(const InvariantSystem& sys,
const StateSpaceType& state_space,
BeliefState& b_x,
const InputBelief& b_u,
const MeasurementBelief& b_z,
typename discrete_sss_traits<InvariantSystem>::time_type t = 0) {
//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) update
// - if the system is linearized, then this will result in an Extended Kalman Filter (EKF) update
BOOST_CONCEPT_ASSERT((pp::TopologyConcept< StateSpaceType >));
BOOST_CONCEPT_ASSERT((InvariantDiscreteSystemConcept<InvariantSystem, StateSpaceType>));
BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<BeliefState>));
BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<InputBelief>));
BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<MeasurementBelief>));
typedef typename discrete_sss_traits<InvariantSystem>::point_type StateType;
typedef typename discrete_sss_traits<InvariantSystem>::output_type OutputType;
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 invariant_system_traits<InvariantSystem>::invariant_frame_type InvarFrame;
typedef typename invariant_system_traits<InvariantSystem>::invariant_error_type InvarErr;
typedef typename invariant_system_traits<InvariantSystem>::invariant_correction_type InvarCorr;
typename discrete_linear_sss_traits<InvariantSystem>::matrixC_type C;
typename discrete_linear_sss_traits<InvariantSystem>::matrixD_type D;
StateType x = b_x.get_mean_state();
MatType P = b_x.get_covariance().get_matrix();
sys.get_output_function_blocks(C, D, state_space, t, x, b_u.get_mean_state());
vect_n<ValueType> e =
to_vect<ValueType>(sys.get_invariant_error(state_space, x, b_u.get_mean_state(), b_z.get_mean_state(), t + sys.get_time_step()));
mat< ValueType, mat_structure::rectangular, mat_alignment::column_major > CP = C * P;
mat< ValueType, mat_structure::symmetric > S(CP * transpose_view(C) + b_z.get_covariance().get_matrix());
linsolve_Cholesky(S,CP);
mat< ValueType, mat_structure::rectangular, mat_alignment::row_major > K(transpose_view(CP));
b_x.set_mean_state( sys.apply_correction(state_space, x, from_vect<InvarCorr>(K * e), b_u.get_mean_state(), t + sys.get_time_step()) );
InvarFrame W = sys.get_invariant_posterior_frame(state_space, x, b_x.get_mean_state(), b_u.get_mean_state(), t + sys.get_time_step());
b_x.set_covariance( CovType( MatType( W * ((mat< ValueType, mat_structure::identity>(K.get_row_count()) - K * C) * P) * transpose_view(W) ) ) );
};
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_update(const System& sys,
const StateSpaceType& state_space,
BeliefState& b_x,
const InputBelief& b_u,
const MeasurementBelief& b_z,
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) update
// - if the system is linearized, then this will result in an Extended Kalman Filter (EKF) update
BOOST_CONCEPT_ASSERT((DiscreteSSSConcept< System, StateSpaceType >));
BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<BeliefState>));
BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<InputBelief>));
BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<MeasurementBelief>));
using std::sqrt;
typedef typename discrete_sss_traits<System>::point_type StateType;
typedef typename discrete_sss_traits<System>::output_type OutputType;
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<MeasurementBelief>::covariance_type OutputCovType;
typedef typename covariance_mat_traits< OutputCovType >::matrix_type OutputMatType;
StateType x = b_x.get_mean_state();
const MatType& P = b_x.get_covariance().get_matrix();
const OutputMatType& R = b_z.get_covariance().get_matrix();
SizeType N = P.get_row_count();
SizeType M = R.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)) = R;
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-Posteriori Covariance P, in UKF update 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));
vect_n< OutputType > Y_a(1 + 2 * (N + M));
X_a[0] = x;
Y_a[0] = sys.get_output(state_space, x, b_u.get_mean_state(), t);
for(SizeType j = 0; j < N+M; ++j) {
X_a[1+2*j] = x;
X_a[2+2*j] = x;
for(SizeType i = 0; i < N; ++i) {
X_a[1+2*j][i] += gamma * L_p(i,j);
X_a[2+2*j][i] -= gamma * L_p(i,j);
};
OutputType z_right = b_z.get_mean_state();
OutputType z_left = b_z.get_mean_state();
for(SizeType i = 0; i < M; ++i) {
z_right[i] = gamma * L_p(N+i,j);
z_left[i] = -gamma * L_p(N+i,j);
};
Y_a[1 + 2*j] = sys.get_output(state_space, X_a[1+2*j], b_u.get_mean_state(), t) + z_right;
Y_a[2 + 2*j] = sys.get_output(state_space, X_a[2+2*j], b_u.get_mean_state(), t) + z_left;
};
gamma = ValueType(1) / (ValueType(N+M) + lambda);
OutputType z_p = (lambda * gamma) * Y_a[0];
for(SizeType j = 0; j < N + M; ++j)
z_p += (0.5 * gamma) * (Y_a[1+2*j] + Y_a[2+2*j]);
for(SizeType j = 0; j < 1 + 2 * (N + M); ++j)
Y_a[j] -= z_p;
mat<ValueType, mat_structure::symmetric> P_zz(z_p.size());
ValueType W_c = (lambda * gamma + ValueType(1) - alpha*alpha + beta);
for(SizeType i = 0; i < M; ++i)
for(SizeType j = i; j < M; ++j)
P_zz(i,j) = W_c * Y_a[0][i] * Y_a[0][j];
W_c = ValueType(0.5) * gamma;
for(SizeType k = 1; k < 1 + 2 * (N + M); ++k)
for(SizeType i = 0; i < M; ++i)
for(SizeType j = i; j < M; ++j)
P_zz(i,j) += W_c * Y_a[k][i] * Y_a[k][j];
mat<ValueType, mat_structure::rectangular> P_xz_t(z_p.size(), N);
for(SizeType k = 1; k < 1 + 2 * (N + M); ++k)
for(SizeType i = 0; i < N; ++i)
for(SizeType j = i; j < M; ++j)
P_xz_t(j,i) += W_c * (X_a[k][i] - x[i]) * Y_a[k][j];
mat<ValueType, mat_structure::rectangular> Kt(P_xz_t);
try {
linsolve_Cholesky(P_zz,Kt);
} catch(singularity_error&) {
//use SVD instead.
mat<ValueType, mat_structure::square> Pzz_pinv(P_zz.get_row_count());
pseudoinvert_SVD(P_zz,Pzz_pinv);
Kt = Pzz_pinv * Kt;
RK_WARNING("A-Posteriori Measurement Covariance Pzz, in UKF update is singular, SVD was used, but this could hide a flaw in the system's setup.");
throw singularity_error("'A-Posteriori Measurement Covariance Pzz, in UKF update'");
};
b_x.set_mean_state( state_space.adjust(x, (b_z.get_mean_state() - z_p) * Kt) );
b_x.set_covariance( CovType( MatType( P - transpose_view(Kt) * P_xz_t ) ) );
};
