Bug #353 » 0001-fix-missing-scalar-in-Jacobian-of-f-theta-q.patch
| codels/ukf_state.cc | ||
|---|---|---|
|
qx*qy, (1+qw) + qy*qy, qy*qz,
|
||
|
qx*qz, qy*qz, (1+qw) + qz*qz,
|
||
|
-(1+qw)*qx, -(1+qw)*qy, -(1+qw)*qz;
|
||
|
J.block<4, 3>(3, 3) *= 0.25;
|
||
|
return J * cov.block<6, 6>(0, 0) * J.transpose();
|
||
|
}
|
||