12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Improved simple helicopter

release/4.3a0
dellaert 2014-12-26 00:53:36 +01:00
parent 625ebca72a
commit aaf4588f20
1 changed files with 15 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -46,29 +46,26 @@ public:
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
if (H1 || H2 || H3) { Matrix6 D_exphxi_xi;
Matrix6 D_Xi_3; Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0);
Pose3 Xi = traits_x<Pose3>::Expmap(h_ * xik, D_Xi_3);
Matrix6 D_gkxi_2, D_gkxi_Xi; Matrix6 D_gkxi_gk, D_gkxi_exphxi;
Pose3 gkxi = gk.compose(Xi, D_gkxi_2, D_gkxi_Xi); Pose3 gkxi = gk.compose(exphxi, D_gkxi_gk, H3 ? &D_gkxi_exphxi : 0);
Matrix6 D_hx_1, D_hx_gkxi; Matrix6 D_hx_gk1, D_hx_gkxi;
Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_1); Pose3 hx = gkxi.between(gk1, (H2 || H3) ? &D_hx_gkxi : 0, H1 ? &D_hx_gk1 : 0);
Matrix6 D_e_hx; Matrix6 D_log_hx;
Vector6 error = traits_x<Pose3>::Logmap(hx, D_e_hx); Vector error = Pose3::Logmap(hx, D_log_hx);
if (H1) *H1 = D_e_hx * D_hx_1;
if (H2) *H2 = D_e_hx * D_hx_gkxi * D_gkxi_2;
if (H3) *H3 = D_e_hx * D_gkxi_Xi * D_Xi_3;
return error; if (H1) *H1 = D_log_hx * D_hx_gk1;
} else { if (H2 || H3) {
Pose3 Xi = Pose3::Expmap(h_ * xik); Matrix6 D_log_gkxi = D_log_hx * D_hx_gkxi;
Pose3 gkxi = gk.compose(Xi); if (H2) *H2 = D_log_gkxi * D_gkxi_gk;
Pose3 hx = gkxi.between(gk1); if (H3) *H3 = D_log_gkxi * D_gkxi_exphxi * D_exphxi_xi * h_;
return Pose3::Logmap(hx);
} }
return error;
} }
}; };