diff options
| author | Martin Miller | 2017-07-07 04:35:29 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-07-07 04:35:29 -0500 |
| commit | ed16dd266029d220109dcbc37c43922b1bbab3f1 (patch) | |
| tree | ad3404d79e3325257b5dd807fbdded472c60d92d | |
| parent | e22f2b6ce5cea845004614d62522e846179dff1b (diff) | |
| parent | 8d73fd6f27ff524be88c2a2e06a67e22f47d28b9 (diff) | |
| download | refslam-ed16dd266029d220109dcbc37c43922b1bbab3f1.zip refslam-ed16dd266029d220109dcbc37c43922b1bbab3f1.tar.gz | |
Merge remote-tracking branch 'origin/732017' into 732017
| -rw-r--r-- | src/feature.cpp | 6 | ||||
| -rw-r--r-- | src/main.cpp | 2 | ||||
| -rw-r--r-- | src/state.cpp | 2 |
3 files changed, 5 insertions, 5 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index 95a5366..82ccc63 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -161,9 +161,9 @@ Feature::R ( ) { Matrix<double,6,6> R; R = Matrix<double,6,6>::Identity(); - R.block<2,2>(0,0) *= VIEW_NOISE; - R.block<2,2>(2,2) *= INITIAL_VIEW_NOISE; - R.block<2,2>(4,4) *= REFLECTION_VIEW_NOISE; + R.block<2,2>(0,0) *= view_noise; + R.block<2,2>(2,2) *= initial_view_noise; + R.block<2,2>(4,4) *= reflection_view_noise; return R; } /* ----- end of method Feature::R ----- */ diff --git a/src/main.cpp b/src/main.cpp index b72c0f1..31222ca 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -133,7 +133,7 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1; Matrix3d pcov = Renuned*msg.covariance.position*Renuned.transpose(); pcov(2,2) *= 10; - mu.position_covariance(pcov); + //mu.position_covariance(pcov); mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw); #if STATESIZE==13 Matrix3d Patt; diff --git a/src/state.cpp b/src/state.cpp index 8b1ff3f..b521eff 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -21,7 +21,7 @@ State::State ( ) { body = new Body; P = Matrix<double,Dynamic,Dynamic>::Zero(STATESIZE,STATESIZE); - P.block<3,3>(6,6) = covbias*Matrix3d::Identity(); + P.block<3,3>(STATESIZE-3,STATESIZE-3) = covbias*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ |