Help needed to understand the covariance matrix, P

[SrijaneeBiswas]

Hello HKU Team,

I'm following up on issues #185 and #215 posted earlier by my colleague. We're trying to get more insight into the structure of the covariance matrix, P, particularly to understand the following questions:

1) If the order of the states is as defined in the state_ikfom manifold in use-ikfom.hpp, why is the order of pos and rot reversed in laserMapping.cpp? 2) Following the same notation above, should P(12,12) to P(14,14) be the covariance for the velocity states? We are trying to publish the velocity covariance as well so that we can fuse it into the FCU. 3) To understand the order of states in the P matrix, we tried to break the system in SITL by flying our UAV in a completely featureless environment. Studying the rosbag data of the P matrix, we realized that the covariance hardly changed (in the order of 10^-8 across all the states).

It'd be great if you could help us understand the same. Thanks in advance!

Regards, Srijanee

[Ecstasy-EC]

The state definition should follow that in use-ikfom.hpp. Since we never use the covariance matrix in our real-world application, we did not test the publishing code, and we disabled the covariance publish to save onboard transmitting bandwidth. You might modify it by yourself for your usage.

As for the small value of the covariance matrix, I would suggest reaching @Joanna-HE for help.

[SrijaneeBiswas]

@Ecstasy-EC thanks for your prompt response.

Regarding your answer, if the state definition follows use-ikfom.hpp, shouldn't the following lines be reversed?

for (int i = 0; i < 6; i ++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3); odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4); odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5); odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0); odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1); odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2); }

i.e., instead be,

for (int i = 0; i < 6; i ++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i*6 + 0] = P(k, 0); odomAftMapped.pose.covariance[i*6 + 1] = P(k, 1); odomAftMapped.pose.covariance[i*6 + 2] = P(k, 2); odomAftMapped.pose.covariance[i*6 + 3] = P(k, 3); odomAftMapped.pose.covariance[i*6 + 4] = P(k, 4); odomAftMapped.pose.covariance[i*6 + 5] = P(k, 5); }

Let me know if there's some gap in my understanding.

Thanks in advance, Srijanee

[SrijaneeBiswas]

@Joanna-HE as @Ecstasy-EC directed me to you, could you help in better understanding of Point 3 above?

Regards, Srijanee

[Ecstasy-EC]

@Ecstasy-EC thanks for your prompt response.

Regarding your answer, if the state definition follows use-ikfom.hpp, shouldn't the following lines be reversed?

for (int i = 0; i < 6; i ++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3); odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4); odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5); odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0); odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1); odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2); }

i.e., instead be,

for (int i = 0; i < 6; i ++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i*6 + 0] = P(k, 0); odomAftMapped.pose.covariance[i*6 + 1] = P(k, 1); odomAftMapped.pose.covariance[i*6 + 2] = P(k, 2); odomAftMapped.pose.covariance[i*6 + 3] = P(k, 3); odomAftMapped.pose.covariance[i*6 + 4] = P(k, 4); odomAftMapped.pose.covariance[i*6 + 5] = P(k, 5); }

Let me know if there's some gap in my understanding.

Thanks in advance, Srijanee

The sequence you provide seems to be correct. However, I am not 100% sure since I did not involve in this part of the code. It might be developed before we added the ikfom and used the new state definition.

[SrijaneeBiswas]

@Ecstasy-EC Another follow-up question is if you're not using the covariance matrix, how are you determining the accuracy of your state estimates?

[Ecstasy-EC]

@Ecstasy-EC Another follow-up question is if you're not using the covariance matrix, how are you determining the accuracy of your state estimates?

The overall accuracy is evaluated offline by comparing trajectories with the ground truth (if we have one) or the end-to-end error after returning to the same starting point. If you are talking about evaluating the accuracy on the fly, theoretically speaking, it is supposed to use the covariance for evaluation. However, as you mentioned in previous issues about the scale of the covariance matrix, we have yet to find a good way to evaluate the accuracy on the fly. Our team has some guys working on it but have not come up with good solutions.

[SrijaneeBiswas]

@Ecstasy-EC Another follow-up question is if you're not using the covariance matrix, how are you determining the accuracy of your state estimates?

The overall accuracy is evaluated offline by comparing trajectories with the ground truth (if we have one) or the end-to-end error after returning to the same starting point. If you are talking about evaluating the accuracy on the fly, theoretically speaking, it is supposed to use the covariance for evaluation. However, as you mentioned in previous issues about the scale of the covariance matrix, we have yet to find a good way to evaluate the accuracy on the fly. Our team has some guys working on it but have not come up with good solutions.

Thanks for the response. Would it be possible for you to connect us to the team or the main developer working on the covariance part? It'd probably give us clarity on how P is calculated and exchange ideas so we can evaluate the accuracy on the fly.

Regards, Srijanee

[Ecstasy-EC]

@Ecstasy-EC Another follow-up question is if you're not using the covariance matrix, how are you determining the accuracy of your state estimates?

The overall accuracy is evaluated offline by comparing trajectories with the ground truth (if we have one) or the end-to-end error after returning to the same starting point. If you are talking about evaluating the accuracy on the fly, theoretically speaking, it is supposed to use the covariance for evaluation. However, as you mentioned in previous issues about the scale of the covariance matrix, we have yet to find a good way to evaluate the accuracy on the fly. Our team has some guys working on it but have not come up with good solutions.

Thanks for the response. Would it be possible for you to connect us to the team or the main developer working on the covariance part? It'd probably give us clarity on how P is calculated and exchange ideas so we can evaluate the accuracy on the fly.

Regards, Srijanee

I believe @Joanna-HE is the most suitable one to help you. I've contacted her but she's been busy these days. She will give you a quick response a.s.a.p.

[Joanna-HE]

@Ecstasy-EC thanks for your prompt response.

Regarding your answer, if the state definition follows use-ikfom.hpp, shouldn't the following lines be reversed?

for (int i = 0; i < 6; i ++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3); odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4); odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5); odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0); odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1); odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2); }

i.e., instead be,

for (int i = 0; i < 6; i ++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i*6 + 0] = P(k, 0); odomAftMapped.pose.covariance[i*6 + 1] = P(k, 1); odomAftMapped.pose.covariance[i*6 + 2] = P(k, 2); odomAftMapped.pose.covariance[i*6 + 3] = P(k, 3); odomAftMapped.pose.covariance[i*6 + 4] = P(k, 4); odomAftMapped.pose.covariance[i*6 + 5] = P(k, 5); }

Let me know if there's some gap in my understanding.

Thanks in advance, Srijanee

Hi Srijanee,

Firstly, the state definition follows use-ikfom.hpp. Secondly, why did you modify the odomAftMapped.pose.covariance to the one shown in this commit? If you agree that the state sequence in odomAftMapped.pose is rot as first and pos as second, then the original settings of FAST-LIO2 are right.

[Joanna-HE]

Since I am not sure about the sequence of the state of odomAftMapped.pose. As searched in the Official documentation, the state sequence of odomAftMapped.pose is (pos, rot), then the original code of FAST-LIO2 is wrong. And the modified result shown in your commit is also wrong. The right one should be: for (int i = 0; i < 6; i ++) {
odomAftMapped.pose.covariance[i6 + 0] = P(i, 0);
odomAftMapped.pose.covariance[i6 + 1] = P(i, 1); odomAftMapped.pose.covariance[i6 + 2] = P(i, 2);
odomAftMapped.pose.covariance[i6 + 3] = P(i, 3);
odomAftMapped.pose.covariance[i6 + 4] = P(i, 4);
odomAftMapped.pose.covariance[i6 + 5] = P(i, 5);
}

[SrijaneeBiswas]

Since I am not sure about the sequence of the state of odomAftMapped.pose. As searched in the Official documentation, the state sequence of odomAftMapped.pose is (pos, rot), then the original code of FAST-LIO2 is wrong. And the modified result shown in your commit is also wrong. The right one should be: for (int i = 0; i < 6; i ++) { odomAftMapped.pose.covariance[i_6 + 0] = P(i, 0); odomAftMapped.pose.covariance[i_6 + 1] = P(i, 1); odomAftMapped.pose.covariance[i_6 + 2] = P(i, 2); odomAftMapped.pose.covariance[i_6 + 3] = P(i, 3); odomAftMapped.pose.covariance[i_6 + 4] = P(i, 4); odomAftMapped.pose.covariance[i_6 + 5] = P(i, 5); }

@Joanna-HE you're right. I realized the error right after posting it. Thanks for your support.

Also, I want to confirm if the following understanding is correct - going by the state definition in use-ikfom.hpp, P(12, 12) to P(14, 14) should be the linear velocity state covariances, right?

[Joanna-HE]

Yes, that is right.

---

寄件者: sbiswas2607 @.> 寄件日期: 2023年4月11日 下午 01:03 收件者: hku-mars/FAST_LIO @.> 副本: @. @.>; Mention @.***> 主旨: Re: [hku-mars/FAST_LIO] Help needed to understand the covariance matrix, P (Issue #233)

Since I am not sure about the sequence of the state of odomAftMapped.pose. As searched in the Official documentation, the state sequence of odomAftMapped.pose is (pos, rot), then the original code of FAST-LIO2 is wrong. And the modified result shown in your commit is also wrong. The right one should be: for (int i = 0; i < 6; i ++) { odomAftMapped.pose.covariance[i_6 + 0] = P(i, 0); odomAftMapped.pose.covariance[i_6 + 1] = P(i, 1); odomAftMapped.pose.covariance[i_6 + 2] = P(i, 2); odomAftMapped.pose.covariance[i_6 + 3] = P(i, 3); odomAftMapped.pose.covariance[i_6 + 4] = P(i, 4); odomAftMapped.pose.covariance[i_6 + 5] = P(i, 5); }

@Joanna-HEhttps://github.com/Joanna-HE you're right. I realized the error right after posting it. Thanks for your support.

Also, I want to confirm if the following understanding is correct - going by the state definition in use-ikfom.hpp, P(12, 12) to P(14, 14) should be the linear velocity state covariances, right?

— Reply to this email directly, view it on GitHubhttps://github.com/hku-mars/FAST_LIO/issues/233#issuecomment-1502692194, or unsubscribehttps://github.com/notifications/unsubscribe-auth/ANJUIXR5RDZCR35JRYQ4LVTXATQ3FANCNFSM6AAAAAAWRHWYPY. You are receiving this because you were mentioned.Message ID: @.***>

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.