H matrix of body_vel sensor

[loechner]

Hi,

I checked the H matrix of the body_vel sensor. Using the results of Wheeler, M. and Ikeuchi, K. "Iterative estimation of rotation and translation using the quaternion", I get H_old.block(0,6,3,3) = - 2 * C_ci*vec1_sk; instead of H_old.block(0,6,3,3) = C_ci*vec1_sk; and H_old.block(0,19,3,3) = - 2 * pic_sk - 2 * vec2_sk; instead of H_old.block(0,19,3,3) = pic_sk+vec2_sk;.
Perhaps this is a typing mistake? However, changing these entries, I can't recognize any difference on the filter outputs.

Additionally, I think the derivative with respect to b_w_ is missing in H. Adding them makes the filter converge much better:

Eigen::Matrix<double,3,1> x_cross_p_ci_, y_cross_p_ci_, z_cross_p_ci_;
x_cross_p_ci_ << 0.0, -p_ci_(2), p_ci_(1);
y_cross_p_ci_ << p_ci_(2), 0.0, -p_ci_(0);
z_cross_p_ci_ << -p_ci_(1), p_ci_(0), 0.0;

H_old.block<3, 1>(0, 9) = - C_e_i * x_cross_p_ci_ * scale;  //b_w_(0)
H_old.block<3, 1>(0, 10) = - C_e_i * y_cross_p_ci_ * scale; //b_w_(1)
H_old.block<3, 1>(0, 11) = - C_e_i * z_cross_p_ci_ * scale; //b_w_(2)`

Can you please check, whether this works for you too?

Cheers
Julian

[stephanweiss]

Julian,

Thanks for this info.
The body_vel branch is not officially supported and is still work under progress. We hope we can soon merge it to master (it essentially only is the addition of the body_vel modules). I will work on this as time allows and will let you know on any progress but can't give you any roadmap.
If you have a wheel odometry module working (i.e. ssf with IMU and wheel odometry) I would like to encourage you to submit your module to the framework. This way we can build a library of sensor modules for the community.

Best
Stephan

---

From: Julian Löchner [notifications@github.com]
Sent: Wednesday, August 21, 2013 7:26 AM
To: ethz-asl/ethzasl_sensor_fusion
Subject: [ethzasl_sensor_fusion] H matrix of body_vel sensor (#12)

Hi,

I checked the H matrix of the body_vel sensor. Using the results of Wheeler, M. and Ikeuchi, K. "Iterative estimation of rotation and translation using the quaternion", I get H_old.block(0,6,3,3) = - 2 * C_ci_vec1_sk; instead of H_old.block(0,6,3,3) = C_ci_vec1_sk; and H_old.block(0,19,3,3) = - 2 * pic_sk - 2 * vec2_sk; instead of H_old.block(0,19,3,3) = pic_sk+vec2_sk;.
Perhaps this is a typing mistake? However, changing these entries, I can't recognize any difference on the filter outputs.

Additionally, I think the derivative with respect to b_w_ is missing in H. Adding them makes the filter converge much better:

Eigen::Matrix<double,3,1> x_cross_p_ci_, y_cross_p_ci_, z_cross_p_ci_;
x_cross_p_ci_ << 0.0, -p_ci_(2), p_ci_(1);
y_cross_p_ci_ << p_ci_(2), 0.0, -p_ci_(0);
z_cross_p_ci_ << -p_ci_(1), p_ci_(0), 0.0;

H_old.block<3, 1>(0, 9) = - C_e_i * x_cross_p_ci_ * scale; //b_w_(0)
H_old.block<3, 1>(0, 10) = - C_e_i * y_cross_p_ci_ * scale; //b_w_(1)
H_old.block<3, 1>(0, 11) = - C_e_i * z_cross_p_ci_ * scale; //b_w_(2)`

Can you please check, whether this works for you too?

Cheers
Julian

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/12.