/** * Parametrized constructor. * \param aBelief The belief-state of the gaussian distribution. */ gaussian_pdf(const BeliefState& aBelief) : mean_state(aBelief.get_mean_state()), L(aBelief.get_covariance().size()), factor(-1) { const matrix_type& E = aBelief.get_covariance().get_matrix(); try { decompose_Cholesky(E,L); } catch(singularity_error&) { return; }; factor = scalar_type(1); for(size_type i = 0; i < L.get_row_count(); ++i) factor *= scalar_type(6.28318530718) * L(i,i); };
/** * Parametrized constructor. * \param aBelief The belief-state of the gaussian distribution. */ gaussian_pdf(const BeliefState& aBelief) : mean_state(aBelief.get_mean_state()), factor(-1) { decompose_QR(aBelief.get_covariance().get_covarying_block(),QX,RX); decompose_QR(aBelief.get_covariance().get_informing_inv_block(),QY,RY); factor = scalar_type(1); for(size_type i = 0; i < RX.get_row_count(); ++i) factor *= scalar_type(6.28318530718) * RX(i,i) / RY(i,i); };
/** * Parametrized constructor. * \param aBelief The belief-state of the gaussian distribution. */ gaussian_sampler(const BeliefState& aBelief) : mean_state(aBelief.get_mean_state()), L(aBelief.get_covariance().size()) { using std::sqrt; const matrix_type& C = aBelief.get_covariance().get_matrix(); try { decompose_Cholesky(C,L); } catch(singularity_error&) { mat< typename mat_traits<matrix_type>::value_type, mat_structure::diagonal> E(aBelief.get_covariance().size()); mat< typename mat_traits<matrix_type>::value_type, mat_structure::square> U(aBelief.get_covariance().size()), V(aBelief.get_covariance().size()); decompose_SVD(C,U,E,V); for(size_type i = 0; i < aBelief.get_covariance().size(); ++i) E(i,i) = sqrt(E(i,i)); L = U * E; }; };
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_filter_step(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>::matrixA_type A; typename discrete_linear_sss_traits<InvariantSystem>::matrixB_type B; 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(); StateType x_prior = sys.get_next_state(state_space, x, b_u.get_mean_state(), t); sys.get_state_transition_blocks(A, B, state_space, t, t + sys.get_time_step(), x, x_prior, b_u.get_mean_state(), b_u.get_mean_state()); InvarFrame W = sys.get_invariant_prior_frame(state_space, x, x_prior, b_u.get_mean_state(), t + sys.get_time_step()); P = W * (( A * P * transpose_view(A)) + B * b_u.get_covariance().get_matrix() * transpose_view(B)) * transpose_view(W); sys.get_output_function_blocks(C, D, state_space, t + sys.get_time_step(), x_prior, b_u.get_mean_state()); vect_n<ValueType> e = to_vect<ValueType>(sys.get_invariant_error(state_space, x_prior, 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_prior, from_vect<InvarCorr>(K * e), b_u.get_mean_state(), t + sys.get_time_step()) ); W = sys.get_invariant_posterior_frame(state_space, x_prior, 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 invariant_kalman_predict(const InvariantSystem& sys, const StateSpaceType& state_space, BeliefState& b_x, const InputBelief& b_u, 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>)); 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; typename discrete_linear_sss_traits<InvariantSystem>::matrixA_type A; typename discrete_linear_sss_traits<InvariantSystem>::matrixB_type B; StateType x = b_x.get_mean_state(); MatType P = b_x.get_covariance().get_matrix(); StateType x_prior = sys.get_next_state(state_space, x, b_u.get_mean_state(), t); sys.get_state_transition_blocks(A, B, state_space, t, t + sys.get_time_step(), x, x_prior, b_u.get_mean_state(), b_u.get_mean_state()); InvarFrame W = sys.get_invariant_prior_frame(state_space, x, x_prior, b_u.get_mean_state(), t + sys.get_time_step()); P = W * (( A * P * transpose_view(A)) + B * b_u.get_covariance().get_matrix() * transpose_view(B)) * transpose_view(W); b_x.set_mean_state( x_prior ); b_x.set_covariance( CovType( P ) ); };
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_aggregate_kf_step(const InvariantSystem& sys, BeliefState& b, const discrete_sss_traits<InvariantSystem>::input_type& b_u, const discrete_sss_traits<InvariantSystem>::output_type& b_z, typename hamiltonian_mat< typename mat_traits< typename covariance_mat_traits< typename continuous_belief_state_traits<BeliefState>::covariance_type >::matrix_type >::value_type >::type& ScSm, typename hamiltonian_mat< typename mat_traits< typename covariance_mat_traits< typename continuous_belief_state_traits<BeliefState>::covariance_type >::matrix_type >::value_type >::type& Sc, 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 typedef typename discrete_sss_traits<InvariantSystem>::point_type StateType; typedef typename discrete_sss_traits<InvariantSystem>::input_type InputType; 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 mat_traits<MatType>::size_type SizeType; BOOST_CONCEPT_ASSERT((InvariantDiscreteSystemConcept<InvariantSystem>)); BOOST_CONCEPT_ASSERT((ContinuousBeliefStateConcept<BeliefState>)); BOOST_CONCEPT_ASSERT((CovarianceMatrixConcept<SystemNoiseCovariance,InputType>)); BOOST_CONCEPT_ASSERT((CovarianceMatrixConcept<MeasurementNoiseCovariance,OutputType>)); typename discrete_linear_sss_traits<InvariantSystem>::matrixA_type A; typename discrete_linear_sss_traits<InvariantSystem>::matrixB_type B; typename discrete_linear_sss_traits<InvariantSystem>::matrixC_type C; typename discrete_linear_sss_traits<InvariantSystem>::matrixD_type D; typedef typename hamiltonian_mat< ValueType >::type HamilMat; typedef typename hamiltonian_mat< ValueType >::upper HamilMatUp; typedef typename hamiltonian_mat< ValueType >::lower HamilMatLo; typedef typename hamiltonian_mat< ValueType >::upper_left HamilMatUL; typedef typename hamiltonian_mat< ValueType >::upper_right HamilMatUR; typedef typename hamiltonian_mat< ValueType >::lower_left HamilMatLL; typedef typename hamiltonian_mat< ValueType >::lower_right HamilMatLR; typedef typename invariant_system_traits<InvariantSystem>::invariant_frame_type InvFrameType; typedef typename invariant_system_traits<InvariantSystem>::invariant_error_type InvErrorType; typedef typename invariant_system_traits<InvariantDiscreteSystem>::invariant_correction_type InvCorrType; StateType x = b.get_mean_state(); MatType P = b.get_covariance().get_matrix(); sys.get_linear_blocks(A, B, C, D, t, x, b_u.get_mean_state()); SizeType N = A.get_col_count(); x = sys.get_next_state(x,u,t); P = ( A * P * transpose_view(A)) + b_u.get_covariance().get_matrix(); InvErrorType e = sys.get_output_error(x, b_u.get_mean_state(), b_z.get_mean_state(), t + sys.get_time_step()); InvFrameType W = sys.get_invariant_prior_frame(b.get_mean_state(), x, b_u.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.set_mean_state( sys.apply_correction(x, from_vect<InvCorrType>(W * K * e), b_u.get_mean_state(), t + sys.get_time_step()) ); W = sys.get_invariant_posterior_frame(x_prior, b.get_mean_state(), b_u.get_mean_state(), t + sys.get_time_step()) * W; InvFrameType Wt = InvFrameType(transpose_view(W)); b.set_covariance( CovType( MatType( W * (mat< ValueType, mat_structure::identity>(K.get_row_count()) - K * C) * P * transpose_view(W) ) ) ); //TODO Apply the W transform somehow. HamilMat Sc_tmp(HamilMatUp(HamilMatUL(A),HamilMatUR(b_u.get_covariance().get_matrix())),HamilMatLo(HamilMatLL(mat<ValueType,mat_structure::nil>(N)),HamilMatLR(transpose_view(A)))); swap(Sc,Sc_tmp); HamilMat ScSm_tmp(star_product(Sc,HamilMatUp(HamilMatUL(mat<ValueType,mat_structure::identity>(N)),HamilMatUR(mat<ValueType,mat_structure::nil>(N))),HamilMatLo(HamilMatLL( transpose_view(C) * b_z.get_covariance().get_inverse_matrix() * C ),HamilMatLR(mat<ValueType,mat_structure::identity>(N))))); swap(ScSm,ScSm_tmp); };
/** * Parametrized constructor. * \param aBelief The belief-state of the gaussian distribution. */ gaussian_pdf(const BeliefState& aBelief) : mean_state(aBelief.get_mean_state()), E_inv(aBelief.get_covariance().get_inverse_matrix()), factor(-1) { factor = determinant_Cholesky(E_inv); if(fabs(factor) < std::numeric_limits< scalar_type >::epsilon()) { factor = scalar_type(-1); } else { factor = scalar_type(1) / factor; for(size_type i = 0; i < E_inv.get_row_count(); ++i) factor *= scalar_type(6.28318530718); }; };
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 ) ); };
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 ) ) ); };