diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 20 |
1 files changed, 18 insertions, 2 deletions
diff --git a/src/state.cpp b/src/state.cpp index 05c3b30..70dc32c 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -69,7 +69,7 @@ State::H ( const Quaterniond &q, const vector<measurement_t> &z ) int col; Feature *fi; switch ( z[i].z_type ) { - case REFLECTION: + case BOTH: col = rowById(z[i].id); if (col==-1) { fprintf(stderr, "Reflection feature %d not found, quitting.\n", z[i].id); @@ -92,8 +92,20 @@ State::H ( const Quaterniond &q, const vector<measurement_t> &z ) fi = featureById( z[i].id ); h.block<4,STATESIZE>(row,0) = fi->Hx(pos,q).block<4,STATESIZE>(0,0); h.block<4,3>(row,col) = fi->Hy(pos,q).block<4,3>(0,0); + row += 4; break; + case REFLECTION: + col = rowById(z[i].id); + if (col==-1) { + fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id); + exit(1); + } + fi = featureById( z[i].id ); + h.block<4,STATESIZE>(row,0) = fi->Hx(pos,q).block<4,STATESIZE>(2,0); + h.block<4,3>(row,col) = fi->Hy(pos,q).block<4,3>(2,0); + row += 4; + case HEIGHT: h.block<1,STATESIZE>(row,0) = body->H(); row += 1; @@ -120,10 +132,14 @@ State::Hrows ( const vector<measurement_t> &z) int rows = 0; for (auto i=z.begin(); i!=z.end(); ++i) { switch ( i->z_type ) { - case REFLECTION: + case BOTH: rows += 6; break; + case REFLECTION: + rows += 4; + break; + case MONO: rows += 4; break; |