search

mbrossar / ai-imu-dr

AI-IMU Dead-Reckoning
MIT License
817 stars 225 forks source link

H jacobian RINS-W: Robust Inertial Navigation System on Wheels #22

Closed amakurin closed 5 years ago

amakurin commented 5 years ago

Hey! thanks for your work! Excuse me for taking your time with, maybe, a silly question, but i can't figure out derivation of H Jacobian in your "RINS-W: Robust Inertial Navigation System on Wheels". I've got no problems with A and C. But following general method i always got same HVel,V as yours, but without Rt multiplication: [0 I ...] vs [0 R' ...] [Gx 0...] vs [R'*Gx 0 ...] Can't figure out where R' comes from... Will appriciate any help. Although this question is not about this particular code but it is about this particular method (same derivation should be used for velocity update in ai-imu paper) and your answer will be extremely helpful.

Thanks in advance!

amakurin commented 5 years ago

for RIEKF i've got CodeCogsEqn what am i missing here?

mbrossar commented 5 years ago

Hello,

For a ekf, we obtain the Jacobian H of the measurement y as defined as y(x) - y(x_hat) = y(x_hat + e) - y(x_hat) y(x) - y(x_hat) = y(x_hat) + He - y(x_hat) + o(e) y(x) - y(x_hat) = He + o(e) where x is the state, x_hat the estimated state and e the error

Here, the state chi lives in a group but the methodology is the same y(chi) - y(chi_hat) = y(exp(xi)chi_hat) - y(chi_hat) y(chi) - y(chi_hat) = y(chi_hat) + Hxi - y(chi_hat) + o(xi) y(chi) - y(chi_hat) = H*xi + o(xi)

For our case, I look at the velocity, we have : y(chi) = R' * v Developing then you obtain [0 R' ...] (I can develop the calculs if required)

Why you obtain I ? You do the job well for an alternative measurement defined as z(chi) = R_hat y(chi) This is the approach of the original riekf, such that H does not depend on the state (so the name invariant). However, both approaches get exactly the same results : when you define z(chi) = R_hat y(chi), you have also to "rotate" the noise (and so the observation noise matrix), leading to the same results as the first method.

amakurin commented 5 years ago

Thank you very much for such a fast answer. I don't think i got your point fully enough. Could you please give 1-2 statments to start calculus derivation? i'm actually started with y(chi) = R'v y(chi_hat)=R_hat'v_hat Hxi + o(xi)= R'v - R_hat'*v_hat well.. thats it... it doesn't look to be able to lead me further...

mbrossar commented 5 years ago

I go on our calculus derivation

y(chi) = R'v y(chi_hat)=R_hat'v_hat

chi = exp(xi) chi_hat = (I + (xi)^) chi_hat + o(xi) // This is the linearization of the exp() of SE3 Developing inside chi give us chi = [R v; 0 1] R = (I + (xi_R))_x R_hat // ()_x is skew operator v = (I + (xi_R))_x v_hat + xi_v

Thus, y(chi) = ((I + (xi_R))_x R_hat )' * ((I + (xi_R))_x v_hat + xi_v)

Hxi + o(xi)= ((I + (xi_R))_x R_hat )' ((I + (xi_R))_x v_hat + xi_v) - R_hat'v_hat Now, keep only first order term Hxi + o(xi)= R_hat' xi_v

amakurin commented 5 years ago

Yes. Now i see it. Again thank you very much for another way of derivation and for help! Sure this will be helpful for all interested.