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

Added numerical derivative in place of CorrectWith...

release/4.3a0
Frank Dellaert 2016-01-17 15:10:54 -08:00
parent 1790d1dc7e
commit d4bbd1f289
1 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/navigation/AggregateImuReadings.cpp
View File

@ -17,6 +17,8 @@
#include <gtsam/navigation/AggregateImuReadings.h> #include <gtsam/navigation/AggregateImuReadings.h>
#include <gtsam/navigation/functors.h> #include <gtsam/navigation/functors.h>
#include <gtsam/base/numericalDerivative.h>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
@ -60,28 +62,25 @@ Vector9 AggregateImuReadings::UpdateEstimate(
const auto theta = dR(zeta); const auto theta = dR(zeta);
const Matrix3 R = Rot3::Expmap(theta, H).matrix(); const Matrix3 R = Rot3::Expmap(theta, H).matrix();
// NOTE(frank): I believe that D_invHwdt_wdt = H.inverse(), but tests fail :-( Matrix3 D_invHwdt_theta = Z_3x3;
Matrix3 D_invHwdt_theta, D_invHwdt_wdt; if (A) {
Vector3 invHwdt; if (useExactDexpDerivative) {
if (useExactDexpDerivative) { boost::function<Vector3(const Vector3&)> f =
// NOTE(frank): ExpmapDerivative(-theta) = H.transpose() not H.inverse() ! [w_dt](const Vector3& theta) {
invHwdt = correctWithExpmapDerivative( return Rot3::ExpmapDerivative(theta).inverse() * w_dt;
-theta, w_dt, A ? &D_invHwdt_theta : 0, A ? &D_invHwdt_wdt : 0); };
if (A) D_invHwdt_theta *= -1; D_invHwdt_theta = numericalDerivative11<Vector3, Vector3>(f, theta);
} else { } else {
const Matrix3 invH = H.inverse(); // First order (small angle) approximation of derivative of invH*w*dt:
invHwdt = invH * w_dt;
// First order (small angle) approximation of derivative of invH*w*dt:
if (A) {
D_invHwdt_theta = skewSymmetric(-0.5 * w_dt); D_invHwdt_theta = skewSymmetric(-0.5 * w_dt);
D_invHwdt_wdt = invH;
} }
} }
Vector9 zeta_plus; Vector9 zeta_plus;
const double dt2 = 0.5 * dt; const double dt2 = 0.5 * dt;
const Vector3 Radt = R * a_dt; const Vector3 Radt = R * a_dt;
dR(zeta_plus) = dR(zeta) + invHwdt; // theta const Matrix3 invH = H.inverse();
dR(zeta_plus) = dR(zeta) + invH * w_dt; // theta
dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + Radt * dt2; // position dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + Radt * dt2; // position
dV(zeta_plus) = dV(zeta) + Radt; // velocity dV(zeta_plus) = dV(zeta) + Radt; // velocity
@ -96,7 +95,7 @@ Vector9 AggregateImuReadings::UpdateEstimate(
A->block<3, 3>(6, 0) = D_Radt_theta; A->block<3, 3>(6, 0) = D_Radt_theta;
} }
if (B) *B << Z_3x3, R* dt* dt2, R* dt; if (B) *B << Z_3x3, R* dt* dt2, R* dt;
if (C) *C << D_invHwdt_wdt* dt, Z_3x3, Z_3x3; if (C) *C << invH* dt, Z_3x3, Z_3x3;
return zeta_plus; return zeta_plus;
} }