Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

关于ieskf更新的过程疑问 #1

Open
SharkPeppero opened this issue Sep 24, 2024 · 0 comments
Open

关于ieskf更新的过程疑问 #1

SharkPeppero opened this issue Sep 24, 2024 · 0 comments

Comments

@SharkPeppero
Copy link

SharkPeppero commented Sep 24, 2024

for (size_t i = 0; i < m_max_iter; i++)
{
    m_loss_func(m_x, shared_data);
    if (!shared_data.valid)
        break;
    H.setZero();
    b.setZero();
    delta = m_x - predict_x;
    M21D J = M21D::Identity();
    J.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0));
    J.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6));
    H += J.transpose() * m_P.inverse() * J;
    b += J.transpose() * m_P.inverse() * delta;

    H.block<12, 12>(0, 0) += shared_data.H;
    b.block<12, 1>(0, 0) += shared_data.b;

    delta = -H.inverse() * b;

    m_x += delta;
    shared_data.iter_num += 1;

    if (m_stop_func(delta))
        break;
}

M21D L = M21D::Identity();
// L.block<3, 3>(0, 0) = Jr(delta.segment<3>(0));
// L.block<3, 3>(6, 6) = Jr(delta.segment<3>(6));
L.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() - 0.5 * Sophus::SO3d::hat(delta.segment<3>(0));
L.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() - 0.5 * Sophus::SO3d::hat(delta.segment<3>(6));
m_P = L * H.inverse() * L.transpose();

m_loss_func(m_x, shared_data);在这里我们利用雷达点构建了点面残差,做了误差状态量中关于位移以及旋转的残差;

     M21D J = M21D::Identity();
    J.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0));
    J.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6));
    H += J.transpose() * m_P.inverse() * J;
    b += J.transpose() * m_P.inverse() * delta;

    H.block<12, 12>(0, 0) += shared_data.H;
    b.block<12, 1>(0, 0) += shared_data.b;
    
    在这里构建的雅阁比J矩阵请问是什么含义呢,为什么又利用这个雅阁比与误差状态量的矩阵构建残差?
    很困解的是这里的J矩阵推导公式是怎么来的,可以麻烦解释一下吗,或者有什么资料可以参考吗
    ~~~~~~~~希望可以回答^_^
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant