Ft matrix
Closed this issue · 3 comments
In your code in KalmanFilter.hpp line 159:
Ft.block<3, 3>(GlobalState::att_, GlobalState::att_) =
skew(gyr - state_tmp.bw_);
But according to your paper and code in LIO-Mapping, this block is "-skew(gyr - state_tmp.bw_)", Is this a bug?
It is a bug. I have revised the code and test it in all datasets. It seems like it does not has any influences to the result. I think it is because this term is so small compared to other blocks that its effect is not obvious. Anyways, thanks for point it out : )
Also, please notice that there is a new typo in my paper abuot Ft. The derivative of the velocity with respect to the gravity should be a 3x3 identity matrix, while the derivative of the attitude with respect to the gravity should be leaved blank, just like what the code does--Ft.block<3, 3>(GlobalState::vel_, GlobalState::gra_) = M3D::Identity();
It is a bug. I have revised the code and test it in all datasets. It seems like it does not has any influences to the result. I think it is because this term is so small compared to other blocks that its effect is not obvious. Anyways, thanks for point it out : )
Also, please notice that there is a new typo in my paper abuot Ft. The derivative of the velocity with respect to the gravity should be a 3x3 identity matrix, while the derivative of the attitude with respect to the gravity should be leaved blank, just like what the code does--Ft.block<3, 3>(GlobalState::vel_, GlobalState::gra_) = M3D::Identity();
Thank you! It seems to be the same as typos in Joan Sola's paper.
It seems that coefficient A of equ 375 in joan sola is right.
