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) ) ) );
};
void RK_CALL manipulator_dynamics_model::computeStateRate(double aTime,const vect_n<double>& aState, vect_n<double>& aStateRate) {
setJointStates(aState);
doMotion();
clearForce();
doForce();
aStateRate.resize(getJointStatesCount());
unsigned int j = 0;
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->q_dot;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = -(*it)->Rotation[1] * (*it)->AngVelocity; ++j;
aStateRate[j] = (*it)->Rotation[0] * (*it)->AngVelocity; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = (*it)->Velocity[2]; ++j;
aStateRate[j] = (*it)->QuatDot[0]; ++j;
aStateRate[j] = (*it)->QuatDot[1]; ++j;
aStateRate[j] = (*it)->QuatDot[2]; ++j;
aStateRate[j] = (*it)->QuatDot[3]; ++j;
};
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->f;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Torque; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Force[2]; ++j;
aStateRate[j] = (*it)->Torque[0]; ++j;
aStateRate[j] = (*it)->Torque[1]; ++j;
aStateRate[j] = (*it)->Torque[2]; ++j;
};
mat<double,mat_structure::symmetric> Msys(getJointAccelerationsCount());
getMassMatrix(Msys);
try {
mat_vect_adaptor< vect_n<double> > acc_as_mat(aStateRate, getJointAccelerationsCount(), 1, getJointPositionsCount());
linsolve_Cholesky(Msys,acc_as_mat);
} catch(singularity_error& e) { RK_UNUSED(e);
std::stringstream ss; ss << "Mass matrix is singular in the manipulator model '" << getName() << "' at time " << aTime << " seconds.";
throw singularity_error(ss.str());
};
};
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);
};
