WIP: Resolve "Complete state vector new data structure?"

Closes #287 (closed) Closes #293 (closed)

demos/hello_wolf/processor_range_bearing.cpp

@@ -125,14 +125,14 @@ ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2
 
 Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
 {
-    Trf H_w_r = transform(getProblem()->getCurrentState());
+    Trf H_w_r = transform(getProblem()->getState().vector("PO"));
     return H_w_r * H_r_s * lmk_s;
 }
 
 Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
 {
 //    Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
-    Trf H_w_r = transform(getProblem()->getCurrentState());
+    Trf H_w_r = transform(getProblem()->getState().vector("PO"));
     return (H_w_r * H_r_s).inverse() * lmk_w;
 }