From 8d73fd6f27ff524be88c2a2e06a67e22f47d28b9 Mon Sep 17 00:00:00 2001
From: Miller
Date: Fri, 7 Jul 2017 04:34:34 -0500
Subject: fix param and P0 bugs

---
 src/feature.cpp | 6 +++---
 src/main.cpp    | 2 +-
 src/state.cpp   | 2 +-
 3 files changed, 5 insertions(+), 5 deletions(-)

(limited to 'src')

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  ----- */
 
-- 
cgit v1.1