Пример #1
0
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) ) ) );
};
Пример #2
0
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 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 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);
};
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 ) ) );
};