diff options
Diffstat (limited to 'src/feature.cpp')
| -rw-r--r-- | src/feature.cpp | 4 |
1 files changed, 3 insertions, 1 deletions
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 ----- */ |