//iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing.
//the measurement and the measurement model are received in a dynamic manner.
//calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
template<typename measurement_runtime, typename measurementModel_dyn_runtime_share>
void update_iterated_dyn_runtime_share(measurement_runtime z, measurementModel_dyn_runtime_share h) {
int t = 0;
dyn_runtime_share_datastruct<scalar_type> dyn_share;
dyn_share.valid = true;
dyn_share.converge = true;
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;
int dof_Measurement_noise;
for(int i=-1; i<maximum_iter; i++)
{
dyn_share.valid = true;
measurement_runtime h_ = h(x_, dyn_share);
//measurement_runtime z = dyn_share.z;
#ifdef USE_sparse
spMt h_x = dyn_share.h_x.sparseView();
spMt h_v = dyn_share.h_v.sparseView();
spMt R_ = dyn_share.R.sparseView();
#else
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
#endif
dof_Measurement = measurement_runtime::DOF;
dof_Measurement_noise = dyn_share.R.rows();
vectorized_state dx, dx_new;
x_.boxminus(dx, x_propagated);
dx_new = dx;
if(! (dyn_share.valid))
{
continue;
}
P_ = P_propagated;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
int idx = (*it).first;
int dim = (*it).second;
for(int i = 0; i < 3; i++){
seg_SO3(i) = dx(idx+i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for(int i = 0; i < n; i++){
P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));
}
for(int i = 0; i < n; i++){
P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
int idx = (*it).first;
int dim = (*it).second;
for(int i = 0; i < 2; i++){
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for(int i = 0; i < n; i++){
P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));
}
for(int i = 0; i < n; i++){
P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
if(n > dof_Measurement)
{
#ifdef USE_sparse
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
spMt R_temp = h_v * R_ * h_v.transpose();
K_temp += R_temp;
K_ = P_ * h_x.transpose() * K_temp.inverse();
#else
K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
#endif
}
else
{
#ifdef USE_sparse
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
solver.compute(R_);
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
spMt R_in =R_in_temp.sparseView();
spMt K_temp = h_x.transpose() * R_in * h_x;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x.transpose() * R_in;
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v*R*h_v.transpose()).inverse();
K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in;
#endif
}
cov K_x = K_ * h_x;
Eigen::Matrix<scalar_type, measurement_runtime::DOF, 1> innovation;
z.boxminus(innovation, h_);
Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
state x_before = x_;
x_.boxplus(dx_);
dyn_share.converge = true;
for(int i = 0; i < n ; i++)
{
if(std::fabs(dx_[i]) > limit[i])
{
dyn_share.converge = false;
break;
}
}
if(dyn_share.converge) t++;
if(t > 1 || i == maximum_iter - 1)
{
L_ = P_;
std::cout << "iteration time:" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
int idx = (*it).first;
for(int i = 0; i < 3; i++){
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
for(int i = 0; i < int(n); i++){
L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));
}
if(n > dof_Measurement)
{
for(int i = 0; i < dof_Measurement; i++){
K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
}
}
else
{
for(int i = 0; i < n; i++){
K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
}
}
for(int i = 0; i < n; i++){
L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
int idx = (*it).first;
for(int i = 0; i < 2; i++){
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for(int i = 0; i < n; i++){
L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));
}
if(n > dof_Measurement)
{
for(int i = 0; i < dof_Measurement; i++){
K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
}
}
else
{
for(int i = 0; i < n; i++){
K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
}
}
for(int i = 0; i < n; i++){
L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
if(n > dof_Measurement)
{
P_ = L_ - K_*h_x * P_;
}
else
{
P_ = L_ - K_x * P_;
}
return;
}
}
}
详细注释上述代码
扫一扫
916
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
