search

ethz-asl / ethzasl_sensor_fusion

time delay single and multi sensor fusion framework based on an EKF
329 stars 162 forks source link

H matrix of body_vel sensor #12

Open loechner opened 11 years ago

loechner commented 11 years ago

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 bw 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 commented 11 years ago

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 bw is missing in H. Adding them makes the filter converge much better:

Eigen::Matrix<double,3,1> x_cross_pci, y_cross_pci, z_cross_pci; x_cross_pci << 0.0, -pci(2), pci(1); y_cross_pci << pci(2), 0.0, -pci(0); z_cross_pci << -pci(1), pci(0), 0.0;

H_old.block<3, 1>(0, 9) = - C_e_i * x_cross_pci * scale; //bw(0) H_old.block<3, 1>(0, 10) = - C_e_i * y_cross_pci * scale; //bw(1) H_old.block<3, 1>(0, 11) = - C_e_i * z_cross_pci * scale; //bw(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/ethz-asl/ethzasl_sensor_fusion/issues/12.