diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/src/state.cpp b/src/state.cpp index ab79086..47364f1 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -83,7 +83,7 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z ) exit(1); } fi = featureById( z[i].id ); - h.block<4,9>(row,0) = fi->Hx(pos,q); + h.block<4,9>(row,0) = fi->Hx(pos,q).block<4,9>(0,0); break; case HEIGHT: @@ -156,7 +156,7 @@ State::R ( const std::vector<measurement_t> &z ) case MONO: fi = featureById(z[i].id); - r.block<4,4>(row,row) = fi->R(); + r.block<4,4>(row,row) = fi->R().block<4,4>(0,0); row += 4; break; @@ -237,8 +237,8 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) } else if (mt==MONO) { fi = featureById(z[i].id); Matrix<double,4,1> hi, zi; - hi = fi->h(body->ned(), q); - zi = fi->measurement_vector(z[i].source, z[i].reflection); + hi = fi->h(body->ned(), q).segment<4>(0); + zi = fi->measurement_vector(z[i].source); y.segment<4>(row) = zi-hi; row += 4; } else if (mt==HEIGHT) { @@ -338,7 +338,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double features.push_back(f); // Expand P - expandP(f->P0()); + expandP(f->P0(i->z_type)); } return ; } /* ----- end of method State::addFeatures ----- */ |