about how to computer measurements Jacobian

[chenwinki]

Hi, thank you for your really greatful work. I am learing about AI-IMU, and I encountert a problem about how to computer measurements Jacobian. I read the sup.pdf #48 which present a example when the measurement is , but the measurement in AI-IMU is more complicate, I have tried to computer the jacobian for AI-IMU measurement, but failed, could you please show us more detailed steps.
thanks very much.

[mbrossar]

Can you please describe how to try to compute the Jacobian ? En 4 mar. 2021 4:40, en 4:40, chenwinki <notifications@github.com> escribió:

…

Hi, thank you for your really greatful work. I am learing about AI-IMU, and I encountert a problem about how to computer measurements Jacobian. I read the sup.pdf #48 which present a example when the measurement is [<img src="https://latex.codecogs.com/gif.latex?y=R^{T}v" title="y=R^{T}v" />](url), but the measurement in AI-IMU is more complicate, I have tried to computer the jacobian for AI-IMU measurement, but failed, could you please show us more detailed steps. thanks very much. -- You are receiving this because you are subscribed to this thread. Reply to this email directly or view it on GitHub: #57

[chenwinki]

I tried to figure out the sup.pdf file, but I get the formula derivation like this,

but in the sup.pdf flie,

the

whether you used the wrong substitution for v.
looking forward to your reply

[ehsankf]

@mbrossar Along the same lines, I wonder in equation (26) in the paper, if the Jacobian of equation (6) i.e., F with respect to the rotation matrix, i.e., F21 where it is equal to (g)_cross should be (lineacc)_cross where lineacc is the linear acceleration.

[Rajat-Arora]

I tried to figure out the sup.pdf file, but I get the formula derivation like this, but in the sup.pdf flie, the whether you used the wrong substitution for v. looking forward to your reply

Hi, @chenwinki did you figure out how the measurement Jacobian was derived?
Also, did you try to derive other terms of the H jacobian, specifically this B and C, in equation 37 of the research article? (Any leads on how to derive them.)

[robocar2018]

I tried to figure out the sup.pdf file, but I get the formula derivation like this, but in the sup.pdf flie, the whether you used the wrong substitution for v. looking forward to your reply

@chenwinki , I think your derivation is correct, and I got the same derivation as yours. The derivation in the sub.pdf seemed to be wrong.

[robocar2018]

@chenwinki , @Rajat-Arora , below is my derivation result which is implemented in the code. And please refer to the paper of the IEEE version (https://www.researchgate.net/publication/339920913_AI-IMU_Dead-Reckoning), which has an update on the measurement function and its corresponding Jacobian.

The measurement function is updated as

And the corresponding Jacobian H becomes

For H matrix, my derivation has a sign difference on the two submatrics of H.

def update(self, Rot, v, p, b_omega, b_acc, Rot_c_i, t_c_i, P, u, i, measurement_cov):
        Omega = self.skew(u[:3] - b_omega)  # skew of angular velocity
        # orientation of body frame
        Rot_body = Rot.dot(Rot_c_i)
        # velocity in body frame in the imu axis
        v_imu = Rot.T.dot(v)
        v_body = Rot_c_i.T.dot(v_imu + Omega.dot(t_c_i))   # velocity in body frame in the vehicle axis
        # Jacobian w.r.t. car frame
        H_v_imu = self.skew(v_imu + Omega.dot(t_c_i)) 
        H_t_c_i = self.skew(t_c_i)
        HH = np.zeros((3, self.P_dim))  # HH is a 3x21 matrix
        HH[:, 3:6] = Rot_body.T  
        HH[:, 9:12] = Rot_c_i.T.dot(H_t_c_i)
        HH[:, 15:18] = Rot_c_i.T.dot(H_v_imu)  # Jacobian of delta_imu_car_rotation_extrinsic 
        HH[:, 18:21] = Rot_c_i.T.dot(Omega)    # Jacobian of delta__imu_car_translation_extrinsic
        H = HH[1:] # extract the second and third rows from HH, which correspond to lateral and vertical speed components respectively.
        r = - v_body[1:]   # r is the residual between measurement, which is just difference between a 2x1 zero vector and v_body[1:],  
                           # v_body[1] is the lateral speed, v_body[2] is the upward speed
        R = np.diag(measurement_cov)
        # Update the state and its covariance matrix 
        Rot_up, v_up, p_up, b_omega_up, b_acc_up, Rot_c_i_up, t_c_i_up, P_up = \
            self.state_and_cov_update(Rot, v, p, b_omega, b_acc, Rot_c_i, t_c_i, P, H, r, R)
        return Rot_up, v_up, p_up, b_omega_up, b_acc_up, Rot_c_i_up, t_c_i_up, P_up

[Rajat-Arora]

@chenwinki , @Rajat-Arora , below is my derivation result which is implemented in the code. And please refer to the paper of the IEEE version (https://www.researchgate.net/publication/339920913_AI-IMU_Dead-Reckoning), which has an update on the measurement function and its corresponding Jacobian.

The measurement function is updated as

And the corresponding Jacobian H becomes

For H matrix, my derivation has a sign difference on the two submatrics of H.

def update(self, Rot, v, p, b_omega, b_acc, Rot_c_i, t_c_i, P, u, i, measurement_cov):
        Omega = self.skew(u[:3] - b_omega)  # skew of angular velocity
        # orientation of body frame
        Rot_body = Rot.dot(Rot_c_i)
        # velocity in body frame in the imu axis
        v_imu = Rot.T.dot(v)
        v_body = Rot_c_i.T.dot(v_imu + Omega.dot(t_c_i))   # velocity in body frame in the vehicle axis
        # Jacobian w.r.t. car frame
        H_v_imu = self.skew(v_imu + Omega.dot(t_c_i)) 
        H_t_c_i = self.skew(t_c_i)
        HH = np.zeros((3, self.P_dim))  # HH is a 3x21 matrix
        HH[:, 3:6] = Rot_body.T  
        HH[:, 9:12] = Rot_c_i.T.dot(H_t_c_i)
        HH[:, 15:18] = Rot_c_i.T.dot(H_v_imu)  # Jacobian of delta_imu_car_rotation_extrinsic 
        HH[:, 18:21] = Rot_c_i.T.dot(Omega)    # Jacobian of delta__imu_car_translation_extrinsic
        H = HH[1:] # extract the second and third rows from HH, which correspond to lateral and vertical speed components respectively.
        r = - v_body[1:]   # r is the residual between measurement, which is just difference between a 2x1 zero vector and v_body[1:],  
                           # v_body[1] is the lateral speed, v_body[2] is the upward speed
        R = np.diag(measurement_cov)
        # Update the state and its covariance matrix 
        Rot_up, v_up, p_up, b_omega_up, b_acc_up, Rot_c_i_up, t_c_i_up, P_up = \
            self.state_and_cov_update(Rot, v, p, b_omega, b_acc, Rot_c_i, t_c_i, P, H, r, R)
        return Rot_up, v_up, p_up, b_omega_up, b_acc_up, Rot_c_i_up, t_c_i_up, P_up

@robocar2018 thanks for sharing had seen this post of your's in another issue as well. Is it possible for you to share how you got to the result of this derivation?
Could you please share the complete derivation?