diff options
| -rw-r--r-- | src/body.cpp | 6 | ||||
| -rw-r--r-- | src/body.h | 2 | ||||
| -rw-r--r-- | src/feature.cpp | 4 | ||||
| -rw-r--r-- | src/feature.h | 2 | ||||
| -rw-r--r-- | src/state.cpp | 2 |
5 files changed, 10 insertions, 6 deletions
diff --git a/src/body.cpp b/src/body.cpp index f27a62f..e6bfb6c 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -105,7 +105,7 @@ Body::Q (double dt) Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity(); Q *= 800e-6; - Q.block<3,3>(6,6) = dt*dt*5e-8*Matrix<double,3,3>::Identity(); + Q.block<3,3>(6,6) = dt*dt*5e-3*Matrix<double,3,3>::Identity(); return Q; } /* ----- end of method Body::q ----- */ @@ -149,7 +149,7 @@ Body::h ( ) *-------------------------------------------------------------------------------------- */ Matrix<double,9,9> -Body::F ( const Vector3d &ang, const Quaterniond &q ) +Body::F ( const Vector3d &ang, const Quaterniond &q, double dt ) { Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); Matrix<double,3,3> Rbw(q.toRotationMatrix()); @@ -158,6 +158,8 @@ Body::F ( const Vector3d &ang, const Quaterniond &q ) F.block<3,3>(0,3) = Rbw; F.block<3,3>(3,3) = -W; F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity(); + F *= dt; + F += Matrix<double,9,9>::Identity(); return F; } /* ----- end of method Body::F ----- */ @@ -29,7 +29,7 @@ class Body void vel(const Matrix<double,3,1> &v); /* ==================== OPERATORS ======================================= */ - Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q); + Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt); Matrix<double,1,9> H(); Matrix<double,1,1> h(); void motionModel ( const Vector3d &acc, const Vector3d &ang, diff --git a/src/feature.cpp b/src/feature.cpp index 99ea448..2afd5a0 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -225,7 +225,7 @@ Feature::Fx ( ) *-------------------------------------------------------------------------------------- */ Matrix<double,3,3> -Feature::Fy ( const Vector3d &v, const Vector3d &w ) +Feature::Fy ( const Vector3d &v, const Vector3d &w, double dt ) { double y0,y1,y2; double v0,v1,v2; @@ -243,6 +243,8 @@ Feature::Fy ( const Vector3d &v, const Vector3d &w ) F << v0*y2-2*w2*y0+y1*w1, w0+y0*w1, -v1+y0*v0, w0-y1*w2, v0*y2+2*w1*y1-y0*w2, -v2+y1*v0, -w2*y2, w1*y2, -w2*y0+w1*y1+2*v0*y2; + F *= dt; + F += Matrix<double,3,3>::Identity(); return F; } /* ----- end of method Feature::Fy ----- */ diff --git a/src/feature.h b/src/feature.h index f3a47fd..fc8fe2f 100644 --- a/src/feature.h +++ b/src/feature.h @@ -27,7 +27,7 @@ class Feature /* ==================== OPERATORS ======================================= */ Matrix<double,3,9> Fx(); - Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang); + Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt); Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q); diff --git a/src/state.cpp b/src/state.cpp index f12ec77..84aa565 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -71,7 +71,7 @@ State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt) { Matrix<double,9,9> F,Q; Q = body->Q(dt); - F = body->F(ang,q); + F = body->F(ang,q,dt); P = F*P*F.transpose()+Q; // Enforce symmetry P = 0.5*(P+P.transpose()); |