diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h
index caa37a9b90..7715142c38 100644
--- a/include/scrimmage/entity/Entity.h
+++ b/include/scrimmage/entity/Entity.h
@@ -33,22 +33,21 @@
 #ifndef INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
 #define INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
 
-#include <scrimmage/fwd_decl.h>
 #include <scrimmage/common/ID.h>
 #include <scrimmage/entity/Contact.h>
+#include <scrimmage/fwd_decl.h>
 #include <scrimmage/proto/Visual.pb.h>
 #include <scrimmage/pubsub/Message.h>
 
+#include <boost/optional.hpp>
+#include <functional>
+#include <list>
 #include <map>
+#include <memory>
 #include <set>
+#include <string>
 #include <unordered_map>
-#include <list>
 #include <vector>
-#include <string>
-#include <functional>
-#include <memory>
-
-#include <boost/optional.hpp>
 
 namespace scrimmage_proto {
 using ContactVisualPtr = std::shared_ptr<ContactVisual>;
@@ -56,7 +55,7 @@ using ContactVisualPtr = std::shared_ptr<ContactVisual>;
 
 namespace scrimmage {
 
-using Service = std::function<bool (scrimmage::MessageBasePtr, scrimmage::MessageBasePtr&)>;
+using Service = std::function<bool(scrimmage::MessageBasePtr, scrimmage::MessageBasePtr&)>;
 
 typedef std::map<std::string, std::map<std::string, std::string>> AttributeMap;
 
@@ -65,31 +64,22 @@ class Entity : public std::enable_shared_from_this<Entity> {
     /*! \name utilities */
     ///@{
 
-    bool init(AttributeMap &overrides,
-              std::map<std::string, std::string> &info,
-              std::shared_ptr<std::unordered_map<int, int>> &id_to_team_map,
-              std::shared_ptr<std::unordered_map<int, EntityPtr>> &id_to_ent_map,
-              ContactMapPtr &contacts,
-              MissionParsePtr mp,
-              const std::shared_ptr<GeographicLib::LocalCartesian> &proj,
-              int id, int ent_desc_id,
-              PluginManagerPtr plugin_manager,
-              FileSearchPtr &file_search,
-              RTreePtr &rtree,
-              PubSubPtr &pubsub,
-              PrintPtr &printer,
-              TimePtr &time,
-              const ParameterServerPtr &param_server,
-              const GlobalServicePtr &global_services,
-              const std::set<std::string> &plugin_tags,
+    bool init(AttributeMap& overrides, std::map<std::string, std::string>& info,
+              std::shared_ptr<std::unordered_map<int, int>>& id_to_team_map,
+              std::shared_ptr<std::unordered_map<int, EntityPtr>>& id_to_ent_map,
+              ContactMapPtr& contacts, MissionParsePtr mp,
+              const std::shared_ptr<GeographicLib::LocalCartesian>& proj, int id, int ent_desc_id,
+              PluginManagerPtr plugin_manager, FileSearchPtr& file_search, RTreePtr& rtree,
+              PubSubPtr& pubsub, PrintPtr& printer, TimePtr& time,
+              const ParameterServerPtr& param_server, const GlobalServicePtr& global_services,
+              const std::set<std::string>& plugin_tags,
               std::function<void(std::map<std::string, std::string>&)> param_override_func,
               const int& debug_level = 0);
 
-    void print_plugins(std::ostream &out) const;
+    void print_plugins(std::ostream& out) const;
 
-    bool parse_visual(std::map<std::string, std::string> &info,
-                      MissionParsePtr mp,
-                      std::map<std::string, std::string> &overrides);
+    bool parse_visual(std::map<std::string, std::string>& info, MissionParsePtr mp,
+                      std::map<std::string, std::string>& overrides);
 
     void close(double t);
     void collision();
@@ -100,15 +90,15 @@ class Entity : public std::enable_shared_from_this<Entity> {
     void setup_desired_state();
     bool ready();
 
-    bool call_service(MessageBasePtr req, MessageBasePtr &res, const std::string &service_name);
+    bool call_service(MessageBasePtr req, MessageBasePtr& res, const std::string& service_name);
 
-    bool call_service(MessageBasePtr &res, const std::string &service_name) {
+    bool call_service(MessageBasePtr& res, const std::string& service_name) {
         return call_service(std::make_shared<MessageBase>(), res, service_name);
     }
 
     template <class T = MessageBasePtr,
               class = typename std::enable_if<!std::is_same<T, MessageBasePtr>::value, void>::type>
-    bool call_service(MessageBasePtr req, T &res, const std::string &service_name) {
+    bool call_service(MessageBasePtr req, T& res, const std::string& service_name) {
         MessageBasePtr res_base;
         if (call_service(req, res_base, service_name)) {
             res = std::dynamic_pointer_cast<typename T::element_type>(res_base);
@@ -125,27 +115,32 @@ class Entity : public std::enable_shared_from_this<Entity> {
 
     template <class T = MessageBasePtr,
               class = typename std::enable_if<!std::is_same<T, MessageBasePtr>::value, void>::type>
-    bool call_service(T &res, const std::string &service_name) {
+    bool call_service(T& res, const std::string& service_name) {
         return call_service(std::make_shared<MessageBase>(), res, service_name);
     }
     ///@}
 
     /*! \name getters/setters */
     ///@{
-    StatePtr &state();
-    StatePtr &state_truth();
-    std::vector<AutonomyPtr> &autonomies();
-    MotionModelPtr &motion();
-    std::vector<ControllerPtr> &controllers();
+    [[deprecated("Use state_belief() or state_truth() to query state information instead")]]
+    StatePtr& state();
 
-    void set_id(ID &id);
-    ID &id();
+    void set_state_belief(const StatePtr& other);
+    void set_state_belief(const State& other);
+    const std::shared_ptr<const State> state_belief() const;
+    StatePtr& state_truth();
+    std::vector<AutonomyPtr>& autonomies();
+    MotionModelPtr& motion();
+    std::vector<ControllerPtr>& controllers();
+
+    void set_id(ID& id);
+    ID& id();
 
     void set_health_points(int health_points);
     int health_points();
 
     std::shared_ptr<GeographicLib::LocalCartesian> projection();
-    void set_projection(const std::shared_ptr<GeographicLib::LocalCartesian> &proj);
+    void set_projection(const std::shared_ptr<GeographicLib::LocalCartesian>& proj);
 
     void set_mp(MissionParsePtr mp);
     MissionParsePtr mp();
@@ -158,40 +153,44 @@ class Entity : public std::enable_shared_from_this<Entity> {
     void set_visual_changed(bool visual_changed);
     bool visual_changed();
 
-    scrimmage_proto::ContactVisualPtr &contact_visual();
+    scrimmage_proto::ContactVisualPtr& contact_visual();
 
-    std::unordered_map<std::string, SensorPtr> &sensors();
-    std::unordered_map<std::string, SensorPtr> sensors(const std::string &sensor_name);
-    SensorPtr sensor(const std::string &sensor_name);
+    std::unordered_map<std::string, SensorPtr>& sensors();
+    std::unordered_map<std::string, SensorPtr> sensors(const std::string& sensor_name);
+    SensorPtr sensor(const std::string& sensor_name);
 
     // Enables creating services at the entity level
-    std::unordered_map<std::string, Service> &services();
+    std::unordered_map<std::string, Service>& services();
     // Enables creating services at the global level (especially for entity interactions, etc.)
-    std::unordered_map<std::string, Service> &global_services();
+    std::unordered_map<std::string, Service>& global_services();
     // Enables setting these for entity interactions
-    void set_global_services(const GlobalServicePtr &global_services);
+    void set_global_services(const GlobalServicePtr& global_services);
 
-    std::unordered_map<std::string, MessageBasePtr> &properties();
+    std::unordered_map<std::string, MessageBasePtr>& properties();
 
     void set_active(bool active);
     bool active();
 
-    ContactMapPtr &contacts() { return contacts_; }
-    RTreePtr &rtree() { return rtree_; }
+    ContactMapPtr& contacts() {
+        return contacts_;
+    }
+    RTreePtr& rtree() {
+        return rtree_;
+    }
 
-    PluginManagerPtr & plugin_manager() {
+    PluginManagerPtr& plugin_manager() {
         return plugin_manager_;
     }
 
-    FileSearchPtr & file_search() {
+    FileSearchPtr& file_search() {
         return file_search_;
     }
 
-    PubSubPtr & pubsub() {
+    PubSubPtr& pubsub() {
         return pubsub_;
     }
 
-    PrintPtr & printer() {
+    PrintPtr& printer() {
         return printer_;
     }
 
@@ -199,7 +198,9 @@ class Entity : public std::enable_shared_from_this<Entity> {
         return param_server_;
     }
 
-    double radius() { return radius_; }
+    double radius() {
+        return radius_;
+    }
     void set_time_ptr(TimePtr t);
     void set_printer(PrintPtr printer);
 
@@ -208,8 +209,7 @@ class Entity : public std::enable_shared_from_this<Entity> {
  protected:
     ID id_;
 
-    scrimmage_proto::ContactVisualPtr visual_ =
-        std::make_shared<scrimmage_proto::ContactVisual>();
+    scrimmage_proto::ContactVisualPtr visual_ = std::make_shared<scrimmage_proto::ContactVisual>();
 
     std::vector<ControllerPtr> controllers_;
     MotionModelPtr motion_model_;
@@ -226,7 +226,7 @@ class Entity : public std::enable_shared_from_this<Entity> {
 
     RandomPtr random_;
 
-    StatePtr state_;
+    StatePtr state_belief_;
     StatePtr state_truth_;
     std::unordered_map<std::string, MessageBasePtr> properties_;
     std::unordered_map<std::string, SensorPtr> sensors_;
@@ -240,7 +240,7 @@ class Entity : public std::enable_shared_from_this<Entity> {
 
     double radius_ = 1;
 
-    void print(const std::string &msg);
+    void print(const std::string& msg);
     PluginManagerPtr plugin_manager_;
     FileSearchPtr file_search_;
     PubSubPtr pubsub_;
@@ -251,5 +251,5 @@ class Entity : public std::enable_shared_from_this<Entity> {
 };
 
 using EntityPtr = std::shared_ptr<Entity>;
-} // namespace scrimmage
-#endif // INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
+}  // namespace scrimmage
+#endif  // INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_
diff --git a/include/scrimmage/math/State.h b/include/scrimmage/math/State.h
index db7a51de02..bbc50e70b4 100644
--- a/include/scrimmage/math/State.h
+++ b/include/scrimmage/math/State.h
@@ -48,6 +48,7 @@ class State {
     State();
     State(Eigen::Vector3d _pos, Eigen::Vector3d _vel,
           Eigen::Vector3d _ang_vel, Quaternion _quat);
+    State(const State& other);
 
     virtual ~State();
 
diff --git a/python/scrimmage/bindings_src/src/py_openai_env.cpp b/python/scrimmage/bindings_src/src/py_openai_env.cpp
index a45c2887c6..c9ffe9a45a 100644
--- a/python/scrimmage/bindings_src/src/py_openai_env.cpp
+++ b/python/scrimmage/bindings_src/src/py_openai_env.cpp
@@ -237,7 +237,7 @@ void ScrimmageOpenAIEnv::scrimmage_memory_cleanup() {
         e->motion() = nullptr;
         e->controllers().clear();
         e->set_mp(nullptr);
-        e->state() = nullptr;
+        e->state_truth() = nullptr;
         e->contacts() = nullptr;
         e->plugin_manager() = nullptr;
         e->file_search() = nullptr;
diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp
index 1ae63f00cf..0d5ad9c61b 100644
--- a/src/entity/Entity.cpp
+++ b/src/entity/Entity.cpp
@@ -31,32 +31,31 @@
  */
 
 #include <scrimmage/autonomy/Autonomy.h>
-#include <scrimmage/common/Utilities.h>
 #include <scrimmage/common/GlobalService.h>
+#include <scrimmage/common/Utilities.h>
 #include <scrimmage/entity/Entity.h>
-#include <scrimmage/math/State.h>
+#include <scrimmage/entity/EntityPluginHelper.h>
 #include <scrimmage/math/Angles.h>
-#include <scrimmage/motion/MotionModel.h>
+#include <scrimmage/math/State.h>
 #include <scrimmage/motion/Controller.h>
-#include <scrimmage/parse/MissionParse.h>
+#include <scrimmage/motion/MotionModel.h>
 #include <scrimmage/parse/ConfigParse.h>
+#include <scrimmage/parse/MissionParse.h>
 #include <scrimmage/parse/ParseUtils.h>
 #include <scrimmage/plugin_manager/PluginManager.h>
 #include <scrimmage/proto/ProtoConversions.h>
 #include <scrimmage/sensor/Sensor.h>
 #include <scrimmage/simcontrol/SimUtils.h>
-#include <scrimmage/entity/EntityPluginHelper.h>
 
-#include <iostream>
-#include <iomanip>
-#include <memory>
 #include <algorithm>
-
 #include <boost/algorithm/cxx11/none_of.hpp>
 #include <boost/algorithm/string/case_conv.hpp>
-#include <boost/range/algorithm/copy.hpp>
-#include <boost/range/adaptor/transformed.hpp>
 #include <boost/lexical_cast.hpp>
+#include <boost/range/adaptor/transformed.hpp>
+#include <boost/range/algorithm/copy.hpp>
+#include <iomanip>
+#include <iostream>
+#include <memory>
 
 using std::cout;
 using std::endl;
@@ -67,23 +66,15 @@ namespace ba = boost::adaptors;
 
 namespace scrimmage {
 
-bool Entity::init(AttributeMap &overrides,
-                  std::map<std::string, std::string> &info,
-                  std::shared_ptr<std::unordered_map<int, int>> &id_to_team_map,
-                  std::shared_ptr<std::unordered_map<int, EntityPtr>> &id_to_ent_map,
-                  ContactMapPtr &contacts,
-                  MissionParsePtr mp,
-                  const std::shared_ptr<GeographicLib::LocalCartesian> &proj,
-                  int id, int ent_desc_id,
-                  PluginManagerPtr plugin_manager,
-                  FileSearchPtr &file_search,
-                  RTreePtr &rtree,
-                  PubSubPtr &pubsub,
-                  PrintPtr &printer,
-                  TimePtr &time,
-                  const ParameterServerPtr &param_server,
-                  const GlobalServicePtr &global_services,
-                  const std::set<std::string> &plugin_tags,
+bool Entity::init(AttributeMap& overrides, std::map<std::string, std::string>& info,
+                  std::shared_ptr<std::unordered_map<int, int>>& id_to_team_map,
+                  std::shared_ptr<std::unordered_map<int, EntityPtr>>& id_to_ent_map,
+                  ContactMapPtr& contacts, MissionParsePtr mp,
+                  const std::shared_ptr<GeographicLib::LocalCartesian>& proj, int id,
+                  int ent_desc_id, PluginManagerPtr plugin_manager, FileSearchPtr& file_search,
+                  RTreePtr& rtree, PubSubPtr& pubsub, PrintPtr& printer, TimePtr& time,
+                  const ParameterServerPtr& param_server, const GlobalServicePtr& global_services,
+                  const std::set<std::string>& plugin_tags,
                   std::function<void(std::map<std::string, std::string>&)> param_override_func,
                   const int& debug_level) {
     pubsub_ = pubsub;
@@ -117,32 +108,32 @@ bool Entity::init(AttributeMap &overrides,
     ////////////////////////////////////////////////////////////
     // set state
     ////////////////////////////////////////////////////////////
-    if (!state_) {
-        state_ = std::make_shared<State>();
+    if (!state_belief_) {
+        state_belief_ = std::make_shared<State>();
     }
-    state_truth_ = state_;
+    state_truth_ = state_belief_;
 
     double x = get("x", info, 0.0);
     double y = get("y", info, 0.0);
     double z = get("z", info, 0.0);
-    state_->pos() << x, y, z;
+    state_truth_->pos() << x, y, z;
 
     double vx = get("vx", info, 0.0);
     double vy = get("vy", info, 0.0);
     double vz = get("vz", info, 0.0);
-    state_->vel() << vx, vy, vz;
+    state_truth_->vel() << vx, vy, vz;
 
     double sp = get("speed", info, 0.0);
     if (sp > 0 && vx == 0 && vy == 0 && vz == 0) {
-      Eigen::Vector3d relative_vel_vector = Eigen::Vector3d::UnitX()*sp;
-      Eigen::Vector3d vel_vector = state_->quat().rotate(relative_vel_vector);
-      state_->vel() << vel_vector[0], vel_vector[1], vel_vector[2];
+        Eigen::Vector3d relative_vel_vector = Eigen::Vector3d::UnitX() * sp;
+        Eigen::Vector3d vel_vector = state_truth_->quat().rotate(relative_vel_vector);
+        state_truth_->vel() << vel_vector[0], vel_vector[1], vel_vector[2];
     }
 
     double roll = Angles::deg2rad(get("roll", info, 0.0));
     double pitch = Angles::deg2rad(get("pitch", info, 0.0));
     double yaw = Angles::deg2rad(get("heading", info, 0.0));
-    state_->quat().set(roll, pitch, yaw);
+    state_truth_->quat().set(roll, pitch, yaw);
 
     EntityPtr parent = shared_from_this();
 
@@ -156,21 +147,16 @@ bool Entity::init(AttributeMap &overrides,
     // The MissionParser appends the order number to the sensor (e.g., sensor0,
     // sensor1, etc.)
     int sensor_ct = 0;
-    std::string sensor_order_name = std::string("sensor") +
-        std::to_string(sensor_ct);
+    std::string sensor_order_name = std::string("sensor") + std::to_string(sensor_ct);
 
     while (info.count(sensor_order_name) > 0) {
         ConfigParse config_parse;
         std::string sensor_name = info[sensor_order_name];
-        PluginStatus<Sensor> status =
-            plugin_manager->make_plugin<Sensor>("scrimmage::Sensor",
-                                                sensor_name, *file_search,
-                                                config_parse,
-                                                overrides[sensor_order_name],
-                                                plugin_tags);
+        PluginStatus<Sensor> status = plugin_manager->make_plugin<Sensor>(
+            "scrimmage::Sensor", sensor_name, *file_search, config_parse,
+            overrides[sensor_order_name], plugin_tags);
         if (status.status == PluginStatus<Sensor>::cast_failed) {
-            std::cout << "Failed to open sensor plugin: " << sensor_name
-                      << std::endl;
+            std::cout << "Failed to open sensor plugin: " << sensor_name << std::endl;
             return false;
         } else if (status.status == PluginStatus<Sensor>::parse_failed) {
             return false;
@@ -191,8 +177,7 @@ bool Entity::init(AttributeMap &overrides,
             if (it_rpy != overrides[sensor_order_name].end()) {
                 str2container(it_rpy->second, " ", tf_rpy, 3);
             }
-            sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]),
-                                            Angles::deg2rad(tf_rpy[1]),
+            sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]), Angles::deg2rad(tf_rpy[1]),
                                             Angles::deg2rad(tf_rpy[2]));
 
             sensor->set_parent(parent);
@@ -206,8 +191,8 @@ bool Entity::init(AttributeMap &overrides,
             // get loop rate from plugin's params
             auto it_loop_rate = config_parse.params().find("loop_rate");
             if (it_loop_rate != config_parse.params().end()) {
-              const double loop_rate = std::stod(it_loop_rate->second);
-              sensor->set_loop_rate(loop_rate);
+                const double loop_rate = std::stod(it_loop_rate->second);
+                sensor->set_loop_rate(loop_rate);
             }
 
             std::string given_name = sensor_name + std::to_string(sensor_ct);
@@ -230,13 +215,9 @@ bool Entity::init(AttributeMap &overrides,
     bool init_empty_motion_model = true;
     if (info.count("motion_model") > 0) {
         ConfigParse config_parse;
-        PluginStatus<MotionModel> status =
-            plugin_manager->make_plugin<MotionModel>("scrimmage::MotionModel",
-                                                     info["motion_model"],
-                                                     *file_search,
-                                                     config_parse,
-                                                     overrides["motion_model"],
-                                                     plugin_tags);
+        PluginStatus<MotionModel> status = plugin_manager->make_plugin<MotionModel>(
+            "scrimmage::MotionModel", info["motion_model"], *file_search, config_parse,
+            overrides["motion_model"], plugin_tags);
         if (status.status == PluginStatus<MotionModel>::cast_failed) {
             cout << "Failed to open motion model plugin: " << info["motion_model"] << endl;
             return false;
@@ -303,22 +284,17 @@ bool Entity::init(AttributeMap &overrides,
         std::string controller_name = *rit;
 
         ConfigParse config_parse;
-        PluginStatus<Controller> status =
-            plugin_manager_->make_plugin<Controller>("scrimmage::Controller",
-                                                     info[controller_name],
-                                                     *file_search,
-                                                     config_parse,
-                                                     overrides[controller_name],
-                                                     plugin_tags);
+        PluginStatus<Controller> status = plugin_manager_->make_plugin<Controller>(
+            "scrimmage::Controller", info[controller_name], *file_search, config_parse,
+            overrides[controller_name], plugin_tags);
         if (status.status == PluginStatus<Controller>::cast_failed) {
-            std::cout << "Failed to open controller plugin: "
-                      << controller_name << std::endl;
+            std::cout << "Failed to open controller plugin: " << controller_name << std::endl;
             return false;
         } else if (status.status == PluginStatus<Controller>::parse_failed) {
             return false;
         } else if (status.status == PluginStatus<Controller>::loaded) {
             ControllerPtr controller = status.plugin;
-            controller->set_state(state_);
+            controller->set_state(state_belief_);
 
             controller->set_parent(shared_from_this());
             controller->set_time(time_);
@@ -332,8 +308,8 @@ bool Entity::init(AttributeMap &overrides,
             // get loop rate from plugin's params
             auto it_loop_rate = config_parse.params().find("loop_rate");
             if (it_loop_rate != config_parse.params().end()) {
-              const double loop_rate = std::stod(it_loop_rate->second);
-              controller->set_loop_rate(loop_rate);
+                const double loop_rate = std::stod(it_loop_rate->second);
+                controller->set_loop_rate(loop_rate);
             }
 
             // Connect this controller to the motion model if it is the last
@@ -358,21 +334,17 @@ bool Entity::init(AttributeMap &overrides,
             // Verify the VariableIO connection
             if (connect_to_motion_model) {
                 if (!verify_io_connection(controller->vars(), motion_model_->vars())) {
-                    std::cout << "VariableIO Error: "
-                              << std::quoted(controller->name())
+                    std::cout << "VariableIO Error: " << std::quoted(controller->name())
                               << " does not provide inputs required by motion model "
-                              << std::quoted(motion_model_->name())
-                              << ": ";
+                              << std::quoted(motion_model_->name()) << ": ";
                     print_io_error(motion_model_->name(), motion_model_->vars());
                     return false;
                 }
             } else if (not connect_to_motion_model) {
                 if (!verify_io_connection(controller->vars(), controllers_.back()->vars())) {
-                    std::cout << "VariableIO Error: "
-                              << std::quoted(controller->name())
+                    std::cout << "VariableIO Error: " << std::quoted(controller->name())
                               << " does not provide inputs required by next controller "
-                              << std::quoted(controllers_.back()->name())
-                              << ": ";
+                              << std::quoted(controllers_.back()->name()) << ": ";
                     print_io_error(controllers_.back()->name(), controllers_.back()->vars());
                     return false;
                 }
@@ -390,11 +362,10 @@ bool Entity::init(AttributeMap &overrides,
 
     // If the motion model requires any inputs and there are no controllers,
     // this is a VariableIO error.
-    if (motion_model_->vars().input_variable_index().size() > 0 &&
-        controllers_.size() == 0) {
+    if (motion_model_->vars().input_variable_index().size() > 0 && controllers_.size() == 0) {
         std::cout << "VariableIO Error: There are not any controllers that "
-                  << "provide the inputs required by "
-                  << std::quoted(motion_model_->name()) << std::endl;
+                  << "provide the inputs required by " << std::quoted(motion_model_->name())
+                  << std::endl;
         print_io_error(motion_model_->name(), motion_model_->vars());
         std::cout << "If you want to directly pass the outputs from the "
                   << "autonomy to the motion_model, see the DirectController "
@@ -417,11 +388,9 @@ bool Entity::init(AttributeMap &overrides,
     // Create the autonomy plugins from the autonomy_names list.
     for (auto autonomy_name : autonomy_names) {
         auto autonomy = make_autonomy<Autonomy>(
-            info[autonomy_name], plugin_manager, overrides[autonomy_name],
-            parent, state_, id_to_team_map, id_to_ent_map, proj_, contacts,
-            file_search, rtree, pubsub, time, param_server, plugin_tags,
-            param_override_func, controllers_,
-            debug_level);
+            info[autonomy_name], plugin_manager, overrides[autonomy_name], parent, state_belief_,
+            id_to_team_map, id_to_ent_map, proj_, contacts, file_search, rtree, pubsub, time,
+            param_server, plugin_tags, param_override_func, controllers_, debug_level);
 
         if (autonomy) {
             autonomies_.push_back(*autonomy);
@@ -435,18 +404,18 @@ bool Entity::init(AttributeMap &overrides,
 
     // Verify that at least one autonomy provides the inputs to the first
     // controller if the first controller requires some VariableIO input.
-    if (connect_entity && not controllers_.empty() &&
-        controllers_.front()->vars().input_variable_index().size() > 0) {
-        auto verify_io = [&](auto &autonomy) {
-            return verify_io_connection(autonomy->vars(),
-                                        controllers_.front()->vars());};
+    if (connect_entity && not controllers_.empty()
+        && controllers_.front()->vars().input_variable_index().size() > 0) {
+        auto verify_io = [&](auto& autonomy) {
+            return verify_io_connection(autonomy->vars(), controllers_.front()->vars());
+        };
         if (boost::algorithm::none_of(autonomies_, verify_io)) {
             auto out_it = std::ostream_iterator<std::string>(std::cout, ", ");
             std::cout << "VariableIO Error: "
                       << "no autonomies provide inputs required by Controller "
                       << std::quoted(controllers_.front()->name())
                       << ". Add VariableIO output declarations in ";
-            auto get_name = [&](auto &p) {return p->name();};
+            auto get_name = [&](auto& p) { return p->name(); };
             br::copy(autonomies_ | ba::transformed(get_name), out_it);
             std::cout << "as follows " << std::endl;
 
@@ -457,7 +426,7 @@ bool Entity::init(AttributeMap &overrides,
 
     if (not controllers_.empty()) {
         if (autonomies_.empty()) {
-            controllers_.front()->set_desired_state(state_);
+            controllers_.front()->set_desired_state(state_belief_);
         } else {
             controllers_.front()->set_desired_state(autonomies_.front()->desired_state());
         }
@@ -465,9 +434,8 @@ bool Entity::init(AttributeMap &overrides,
     return true;
 }
 
-bool Entity::parse_visual(std::map<std::string, std::string> &info,
-                          MissionParsePtr mp,
-                          std::map<std::string, std::string> &overrides) {
+bool Entity::parse_visual(std::map<std::string, std::string>& info, MissionParsePtr mp,
+                          std::map<std::string, std::string>& overrides) {
     visual_->set_id(id_.id());
     visual_->set_opacity(1.0);
 
@@ -478,15 +446,13 @@ bool Entity::parse_visual(std::map<std::string, std::string> &info,
         return true;
     }
 
-    find_model_properties(it->second, cv_parse,
-                          *file_search_, overrides, visual_,
-                          mesh_found, texture_found);
+    find_model_properties(it->second, cv_parse, *file_search_, overrides, visual_, mesh_found,
+                          texture_found);
 
     // Set the entity color. Use the team color by default
     std::vector<int> color;
     auto it_color = info.find("color");
-    if (it_color != info.end() and
-        str2container(it_color->second, ", ", color, 3)) {
+    if (it_color != info.end() and str2container(it_color->second, ", ", color, 3)) {
     } else {
         set(color, mp->team_info()[id_.team_id()].color);
     }
@@ -495,7 +461,8 @@ bool Entity::parse_visual(std::map<std::string, std::string> &info,
     std::string visual_model = boost::to_upper_copy(info["visual_model"]);
     if (mesh_found) {
         type_ = Contact::Type::MESH;
-        visual_->set_visual_mode(texture_found ? scrimmage_proto::ContactVisual::TEXTURE : scrimmage_proto::ContactVisual::COLOR);
+        visual_->set_visual_mode(texture_found ? scrimmage_proto::ContactVisual::TEXTURE
+                                               : scrimmage_proto::ContactVisual::COLOR);
     } else if (visual_model == std::string("QUADROTOR")) {
         type_ = Contact::Type::QUADROTOR;
         visual_->set_visual_mode(scrimmage_proto::ContactVisual::COLOR);
@@ -514,81 +481,134 @@ bool Entity::parse_visual(std::map<std::string, std::string> &info,
 }
 
 bool Entity::ready() {
-    auto all_ready = [&](auto &rng, auto &func) {
+    auto all_ready = [&](auto& rng, auto& func) {
         return std::all_of(rng.begin(), rng.end(), func);
     };
 
-    auto single_ready = [&](auto &plugin) {return plugin->ready();};
-    auto values_single_ready = [&](auto &kv) {return kv.second->ready();};
+    auto single_ready = [&](auto& plugin) { return plugin->ready(); };
+    auto values_single_ready = [&](auto& kv) { return kv.second->ready(); };
 
-    return all_ready(autonomies_, single_ready)
-        && all_ready(controllers_, single_ready)
-        && all_ready(sensors_, values_single_ready)
-        && motion_model_->ready();
+    return all_ready(autonomies_, single_ready) && all_ready(controllers_, single_ready)
+           && all_ready(sensors_, values_single_ready) && motion_model_->ready();
 }
 
-StatePtr &Entity::state() {return state_;}
-StatePtr &Entity::state_truth() {return state_truth_;}
+StatePtr& Entity::state() {
+    return state_belief_;
+}
 
-std::vector<AutonomyPtr> &Entity::autonomies() {return autonomies_;}
+void Entity::set_state_belief(const StatePtr& other) {
+    if (state_belief_ == state_truth_) {
+        std::cout << "Decoupling State Belief and State Truth. Ensure that you have an explicit "
+                  << "method of updating the state belief.\n";
+        state_belief_ = std::make_shared<State>();
+    }
+    *state_belief_ = *other;
+}
 
-MotionModelPtr &Entity::motion() {return motion_model_;}
+void Entity::set_state_belief(const State& other) {
+    if (state_belief_ == state_truth_) {
+        std::cout << "Decoupling State Belief and State Truth. Ensure that you have an explicit "
+                  << "method of updating the state belief.\n";
+        state_belief_ = std::make_shared<State>();
+    }
+    *state_belief_ = other;
+}
 
-std::vector<ControllerPtr> &Entity::controllers() {
+const std::shared_ptr<const State> Entity::state_belief() const {
+    return state_belief_;
+}
+StatePtr& Entity::state_truth() {
+    return state_truth_;
+}
+
+std::vector<AutonomyPtr>& Entity::autonomies() {
+    return autonomies_;
+}
+
+MotionModelPtr& Entity::motion() {
+    return motion_model_;
+}
+
+std::vector<ControllerPtr>& Entity::controllers() {
     return controllers_;
 }
 
-void Entity::set_id(ID &id) { id_ = id; }
+void Entity::set_id(ID& id) {
+    id_ = id;
+}
 
-ID &Entity::id() { return id_; }
+ID& Entity::id() {
+    return id_;
+}
 
-void Entity::collision() { health_points_ -= 1e9; }
+void Entity::collision() {
+    health_points_ -= 1e9;
+}
 
-void Entity::hit() { health_points_--; }
+void Entity::hit() {
+    health_points_--;
+}
 
-void Entity::set_health_points(int health_points)
-{ health_points_ = health_points; }
+void Entity::set_health_points(int health_points) {
+    health_points_ = health_points;
+}
 
-int Entity::health_points() { return health_points_; }
+int Entity::health_points() {
+    return health_points_;
+}
 
 bool Entity::is_alive() {
     return (health_points_ > 0);
 }
 
 bool Entity::posthumous(double t) {
-    bool any_autonomies =
-        std::any_of(autonomies_.begin(), autonomies_.end(),
-                    [t](AutonomyPtr &a) {return a->posthumous(t);});
+    bool any_autonomies = std::any_of(autonomies_.begin(), autonomies_.end(),
+                                      [t](AutonomyPtr& a) { return a->posthumous(t); });
     return any_autonomies && motion_model_->posthumous(t);
 }
 
-std::shared_ptr<GeographicLib::LocalCartesian> Entity::projection()
-{ return proj_; }
+std::shared_ptr<GeographicLib::LocalCartesian> Entity::projection() {
+    return proj_;
+}
 
-MissionParsePtr Entity::mp() { return mp_; }
+MissionParsePtr Entity::mp() {
+    return mp_;
+}
 
-void Entity::set_mp(MissionParsePtr mp) { mp_ = mp; }
-void Entity::set_random(RandomPtr random) { random_ = random; }
+void Entity::set_mp(MissionParsePtr mp) {
+    mp_ = mp;
+}
+void Entity::set_random(RandomPtr random) {
+    random_ = random;
+}
 
-RandomPtr Entity::random() { return random_; }
+RandomPtr Entity::random() {
+    return random_;
+}
 
-Contact::Type Entity::type() { return type_; }
+Contact::Type Entity::type() {
+    return type_;
+}
 
-void Entity::set_visual_changed(bool visual_changed)
-{ visual_changed_ = visual_changed; }
+void Entity::set_visual_changed(bool visual_changed) {
+    visual_changed_ = visual_changed;
+}
 
-bool Entity::visual_changed() { return visual_changed_; }
+bool Entity::visual_changed() {
+    return visual_changed_;
+}
 
-scrimmage_proto::ContactVisualPtr &Entity::contact_visual()
-{ return visual_; }
+scrimmage_proto::ContactVisualPtr& Entity::contact_visual() {
+    return visual_;
+}
 
-std::unordered_map<std::string, SensorPtr> &Entity::sensors() {
+std::unordered_map<std::string, SensorPtr>& Entity::sensors() {
     return sensors_;
 }
 
-std::unordered_map<std::string, SensorPtr> Entity::sensors(const std::string &sensor_name) {
+std::unordered_map<std::string, SensorPtr> Entity::sensors(const std::string& sensor_name) {
     std::unordered_map<std::string, SensorPtr> out;
-    for (auto &kv : sensors_) {
+    for (auto& kv : sensors_) {
         if (kv.first.find(sensor_name) != std::string::npos) {
             out[kv.first] = kv.second;
         }
@@ -596,47 +616,52 @@ std::unordered_map<std::string, SensorPtr> Entity::sensors(const std::string &se
     return out;
 }
 
-SensorPtr Entity::sensor(const std::string &sensor_name) {
+SensorPtr Entity::sensor(const std::string& sensor_name) {
     std::unordered_map<std::string, SensorPtr> out = sensors(sensor_name);
     return out.empty() ? nullptr : out.begin()->second;
 }
 
-void Entity::set_active(bool active) { active_ = active; }
+void Entity::set_active(bool active) {
+    active_ = active;
+}
 
-bool Entity::active() { return active_; }
+bool Entity::active() {
+    return active_;
+}
 
 void Entity::setup_desired_state() {
     if (controllers_.empty()) return;
 
     auto it = std::find_if(autonomies_.rbegin(), autonomies_.rend(),
-        [&](auto autonomy) {return autonomy->get_is_controlling();});
+                           [&](auto autonomy) { return autonomy->get_is_controlling(); });
 
     if (it != autonomies_.rend()) {
         controllers_.front()->set_desired_state((*it)->desired_state());
     }
 }
 
-std::unordered_map<std::string, Service> &Entity::services() {return services_;}
-std::unordered_map<std::string, Service> &Entity::global_services() {
+std::unordered_map<std::string, Service>& Entity::services() {
+    return services_;
+}
+std::unordered_map<std::string, Service>& Entity::global_services() {
     return global_services_->services();
 }
 
-void Entity::set_global_services(const GlobalServicePtr &global_services) {
+void Entity::set_global_services(const GlobalServicePtr& global_services) {
     global_services_ = global_services;
 }
 
-bool Entity::call_service(scrimmage::MessageBasePtr req,
-        scrimmage::MessageBasePtr &res, const std::string &service_name) {
+bool Entity::call_service(scrimmage::MessageBasePtr req, scrimmage::MessageBasePtr& res,
+                          const std::string& service_name) {
     auto it = services_.find(service_name);
     if (it == services_.end()) {
         // First check for a global service of this name
         bool found = global_services_->call_service(req, res, service_name);
         if (!found) {
-            std::cout << "request for service ("
-                << service_name
-                << ") that does not exist" << std::endl;
+            std::cout << "request for service (" << service_name << ") that does not exist"
+                      << std::endl;
             std::cout << "services are: ";
-            for (auto &kv : services_) {
+            for (auto& kv : services_) {
                 std::cout << kv.first << ", ";
             }
             std::cout << std::endl;
@@ -646,7 +671,7 @@ bool Entity::call_service(scrimmage::MessageBasePtr req,
         }
     }
 
-    Service &service = it->second;
+    Service& service = it->second;
     bool success = service(req, res);
 
     if (!success) {
@@ -657,7 +682,7 @@ bool Entity::call_service(scrimmage::MessageBasePtr req,
     }
 }
 
-void Entity::print(const std::string &msg) {
+void Entity::print(const std::string& msg) {
     std::cout << msg << std::endl;
 }
 
@@ -666,7 +691,7 @@ void Entity::close(double t) {
         autonomy->close_plugin(t);
     }
 
-    for (auto &kv : sensors_) {
+    for (auto& kv : sensors_) {
         kv.second->close_plugin(t);
     }
 
@@ -684,7 +709,7 @@ void Entity::close(double t) {
     mp_ = nullptr;
     proj_ = nullptr;
     random_ = nullptr;
-    state_ = nullptr;
+    state_belief_ = nullptr;
     state_truth_ = nullptr;
     properties_.clear();
     sensors_.clear();
@@ -699,22 +724,25 @@ void Entity::close(double t) {
     time_ = nullptr;
 }
 
-std::unordered_map<std::string, MessageBasePtr> &Entity::properties() {
+std::unordered_map<std::string, MessageBasePtr>& Entity::properties() {
     return properties_;
 }
 
-void Entity::set_time_ptr(TimePtr t) {time_ = t;}
-void Entity::set_printer(PrintPtr printer) { printer_ = printer; }
-
+void Entity::set_time_ptr(TimePtr t) {
+    time_ = t;
+}
+void Entity::set_printer(PrintPtr printer) {
+    printer_ = printer;
+}
 
 // cppcheck-suppress passedByValue
-void Entity::set_projection(const std::shared_ptr<GeographicLib::LocalCartesian> &proj) {
+void Entity::set_projection(const std::shared_ptr<GeographicLib::LocalCartesian>& proj) {
     proj_ = proj;
 }
 
-void Entity::print_plugins(std::ostream &out) const {
+void Entity::print_plugins(std::ostream& out) const {
     out << "----------- Sensor -------------" << endl;
-    for (auto &kv : sensors_) {
+    for (auto& kv : sensors_) {
         out << kv.second->name() << endl;
     }
     out << "---------- Autonomy ------------" << endl;
diff --git a/src/entity/External.cpp b/src/entity/External.cpp
index ed5a264851..cfbb4f32f9 100644
--- a/src/entity/External.cpp
+++ b/src/entity/External.cpp
@@ -196,7 +196,7 @@ bool External::create_entity(const std::string &mission_file,
     entity_->set_random(random);
     entity_->contacts() = contacts;
     entity_->rtree() = rtree;
-    entity_->state() = std::make_shared<State>();
+    entity_->state_truth() = std::make_shared<State>();
 
     call_update_contacts(time_->t());
     auto it = entity_->contacts()->find(entity_id);
@@ -434,10 +434,10 @@ void External::update_ents() {
 
         auto ent = id.id() == entity_->id().id() ? entity_ : std::make_shared<Entity>();
         ent->id() = id;
-        if (ent->state()) {
-            *ent->state() = *kv.second.state();
+        if (ent->state_truth()) {
+            *ent->state_truth() = *kv.second.state();
         } else {
-            ent->state() = kv.second.state();
+            ent->state_truth() = kv.second.state();
         }
         ents_.push_back(ent);
 
diff --git a/src/math/State.cpp b/src/math/State.cpp
index 94605c3981..2b9c6a13c2 100644
--- a/src/math/State.cpp
+++ b/src/math/State.cpp
@@ -31,42 +31,80 @@
  */
 
 #include <scrimmage/common/Utilities.h>
-#include <scrimmage/math/State.h>
 #include <scrimmage/math/Angles.h>
+#include <scrimmage/math/State.h>
 
-#include <iostream>
 #include <iomanip>
+#include <iostream>
 
 namespace scrimmage {
 
-State::State() : pos_(0, 0, 0), vel_(0, 0, 0), ang_vel_(0, 0, 0) {}
+State::State() : pos_(0, 0, 0), vel_(0, 0, 0), ang_vel_(0, 0, 0) {
+}
 
-State::State(Eigen::Vector3d _pos, Eigen::Vector3d _vel,
-             Eigen::Vector3d _ang_vel, Quaternion _quat) :
-    pos_(_pos), vel_(_vel), ang_vel_(_ang_vel), quat_(_quat) {}
+State::State(Eigen::Vector3d _pos, Eigen::Vector3d _vel, Eigen::Vector3d _ang_vel, Quaternion _quat)
+    : pos_(_pos), vel_(_vel), ang_vel_(_ang_vel), quat_(_quat) {
+}
 
-State::~State() {}
+State::State(const State& other)
+    : pos_{other.pos_}, vel_{other.vel_}, ang_vel_{other.ang_vel_}, quat_{other.quat_} {
+}
 
-Eigen::Vector3d &State::pos() {return pos_;}
-Eigen::Vector3d &State::vel() {return vel_;}
-Eigen::Vector3d &State::ang_vel() {return ang_vel_;}
-Quaternion &State::quat() {return quat_;}
-const Eigen::Vector3d &State::pos() const {return pos_;}
-const Eigen::Vector3d &State::vel() const {return vel_;}
-const Eigen::Vector3d &State::ang_vel() const {return ang_vel_;}
-const Quaternion &State::quat() const {return quat_;}
+State::~State() {
+}
+
+Eigen::Vector3d& State::pos() {
+    return pos_;
+}
+Eigen::Vector3d& State::vel() {
+    return vel_;
+}
+Eigen::Vector3d& State::ang_vel() {
+    return ang_vel_;
+}
+Quaternion& State::quat() {
+    return quat_;
+}
+const Eigen::Vector3d& State::pos() const {
+    return pos_;
+}
+const Eigen::Vector3d& State::vel() const {
+    return vel_;
+}
+const Eigen::Vector3d& State::ang_vel() const {
+    return ang_vel_;
+}
+const Quaternion& State::quat() const {
+    return quat_;
+}
 
 // for backwards compatibility
-const Eigen::Vector3d &State::pos_const() const {return pos_;}
-const Eigen::Vector3d &State::vel_const() const {return vel_;}
-const Eigen::Vector3d &State::ang_vel_const() const {return ang_vel_;}
-const Quaternion &State::quat_const() const {return quat_;}
-void State::set_pos(const Eigen::Vector3d &pos) {pos_ = pos;}
-void State::set_vel(const Eigen::Vector3d &vel) {vel_ = vel;}
-void State::set_ang_vel(const Eigen::Vector3d &ang_vel) {ang_vel_ = ang_vel;}
-void State::set_quat(const Quaternion &quat) {quat_ = quat;}
-
-bool State::InFieldOfView(State &other, double fov_width, double fov_height) const {
+const Eigen::Vector3d& State::pos_const() const {
+    return pos_;
+}
+const Eigen::Vector3d& State::vel_const() const {
+    return vel_;
+}
+const Eigen::Vector3d& State::ang_vel_const() const {
+    return ang_vel_;
+}
+const Quaternion& State::quat_const() const {
+    return quat_;
+}
+void State::set_pos(const Eigen::Vector3d& pos) {
+    pos_ = pos;
+}
+void State::set_vel(const Eigen::Vector3d& vel) {
+    vel_ = vel;
+}
+void State::set_ang_vel(const Eigen::Vector3d& ang_vel) {
+    ang_vel_ = ang_vel;
+}
+void State::set_quat(const Quaternion& quat) {
+    quat_ = quat;
+}
+
+bool State::InFieldOfView(State& other, double fov_width, double fov_height) const {
     Eigen::Vector3d rel_pos = this->rel_pos_local_frame(other.pos());
     double az = atan2(rel_pos(1), rel_pos(0));
     double norm_xy = sqrt(pow(rel_pos(0), 2) + pow(rel_pos(1), 2));
@@ -74,7 +112,7 @@ bool State::InFieldOfView(State &other, double fov_width, double fov_height) con
     return std::abs(az) < fov_width / 2 && std::abs(el) < fov_height / 2;
 }
 
-Eigen::Vector3d State::rel_pos_local_frame(Eigen::Vector3d &other) const {
+Eigen::Vector3d State::rel_pos_local_frame(Eigen::Vector3d& other) const {
     return quat_.rotate_reverse(other - pos_);
 }
 
@@ -90,7 +128,7 @@ Eigen::Vector3d State::orient_global_frame() const {
     return quat_.rotate(Eigen::Vector3d::UnitX());
 }
 
-double State::rel_az(const Eigen::Vector3d &other) const {
+double State::rel_az(const Eigen::Vector3d& other) const {
     Eigen::Vector3d diff = other - pos_;
     double az = atan2(diff(1), diff(0));
     return Angles::angle_diff_rad(az, quat_.yaw());
@@ -113,16 +151,15 @@ Eigen::Matrix4d State::tf_matrix(bool enable_translate) {
 }
 
 std::ostream& operator<<(std::ostream& os, const State& s) {
-    const Quaternion &q = s.quat();
-
-    os << "(" << eigen_str(s.pos_, s.output_precision)
-        << "), (" << eigen_str(s.vel_, s.output_precision)
-        << "), (" << eigen_str(s.ang_vel_, s.output_precision)
-        << "), ("
-        << std::setprecision(s.output_precision) << q.roll() << ", "
-        << std::setprecision(s.output_precision) << q.pitch() << ", "
-        << std::setprecision(s.output_precision) << q.yaw() << ")";
+    const Quaternion& q = s.quat();
+
+    os << "(" << eigen_str(s.pos_, s.output_precision) << "), ("
+       << eigen_str(s.vel_, s.output_precision) << "), ("
+       << eigen_str(s.ang_vel_, s.output_precision) << "), ("
+       << std::setprecision(s.output_precision) << q.roll() << ", "
+       << std::setprecision(s.output_precision) << q.pitch() << ", "
+       << std::setprecision(s.output_precision) << q.yaw() << ")";
     return os;
 }
 
-} // namespace scrimmage
+}  // namespace scrimmage
diff --git a/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp b/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp
index d2a7d8a4f8..fd614603b9 100644
--- a/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp
+++ b/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp
@@ -64,7 +64,7 @@ void GoToWaypoint::init(std::map<std::string, std::string> &params) {
 bool GoToWaypoint::step_autonomy(double t, double dt) {
     if (waypoint_status_) {
 
-        sc::State ns = *(parent_->state());
+        sc::State ns = *(parent_->state_belief());
 
         double lat = (waypoint_[2] == "X") ? ns.pos()(0): stod(waypoint_[2]);
         double lon = (waypoint_[3] == "Y") ? ns.pos()(1): stod(waypoint_[3]);
diff --git a/src/plugins/sensor/NoisyState/NoisyState.cpp b/src/plugins/sensor/NoisyState/NoisyState.cpp
index f5d68e51a0..6e254287fd 100644
--- a/src/plugins/sensor/NoisyState/NoisyState.cpp
+++ b/src/plugins/sensor/NoisyState/NoisyState.cpp
@@ -70,8 +70,7 @@ void NoisyState::init(std::map<std::string, std::string> &params) {
 
     // Move the enity's state pointer to use the state instance provided by
     // this plugin.
-    parent_->state() = std::make_shared<State>();
-    *(parent_->state()) = *(parent_->state_truth());
+    parent_->set_state_belief(parent_->state_truth());
 
     return;
 }
@@ -99,7 +98,7 @@ bool NoisyState::step() {
     pub_->publish(msg);
 
     // Update the entity's state.
-    *(parent_->state()) = static_cast<sc::State>(msg->data);
+    parent_->set_state_belief(static_cast<sc::State>(msg->data));
 
     return true;
 }
diff --git a/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp b/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp
index 52346ce30d..c0ea9fff32 100644
--- a/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp
+++ b/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp
@@ -43,7 +43,7 @@ namespace scrimmage {
 namespace sensor {
 
 void RLSimpleSensor::get_observation(double *data, uint32_t beg_idx, uint32_t /*end_idx*/) {
-    data[beg_idx] = parent_->state()->pos()(0);
+    data[beg_idx] = parent_->state_belief()->pos()(0);
     data[beg_idx + 1] = time_->t();
 }
 
diff --git a/src/plugins/sensor/SimpleINS/SimpleINS.cpp b/src/plugins/sensor/SimpleINS/SimpleINS.cpp
index dafcb8f9dd..54b8f2164a 100644
--- a/src/plugins/sensor/SimpleINS/SimpleINS.cpp
+++ b/src/plugins/sensor/SimpleINS/SimpleINS.cpp
@@ -88,8 +88,8 @@ void SimpleINS::init(std::map<std::string, std::string> &params) {
 
     surface_timer_ = sc::get<double>("surface_timer", params, 10);
 
-    parent_->state() = std::make_shared<State>();
-    *(parent_->state()) = *(parent_->state_truth());
+
+    parent_->set_state_belief(parent_->state_truth());
 }
 
 
@@ -190,7 +190,7 @@ bool SimpleINS::step() {
     pub_->publish(msg);
 
     // Update the entity's state.
-    *(parent_->state()) = static_cast<sc::State>(msg->data);
+    parent_->set_state_belief(static_cast<sc::State>(msg->data));
 
     return true;
 }
diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp
index a7c8589e49..c13dded958 100644
--- a/src/simcontrol/SimControl.cpp
+++ b/src/simcontrol/SimControl.cpp
@@ -390,7 +390,7 @@ bool SimControl::generate_entity(const int &ent_desc_id,
     }
 
     ents_.push_back(ent);
-    rtree_->add(ent->state()->pos(), ent->id());
+    rtree_->add(ent->state_truth()->pos(), ent->id());
     contacts_mutex_.lock();
     (*contacts_)[ent->id().id()] =
             Contact(ent->id(), ent->radius(), ent->state_truth(),
@@ -421,7 +421,7 @@ void SimControl::join() {
 void SimControl::create_rtree(const unsigned int& additional_size) {
     rtree_->init(ents_.size() + additional_size);
     for (EntityPtr &ent: ents_) {
-        rtree_->add(ent->state()->pos(), ent->id());
+        rtree_->add(ent->state_truth()->pos(), ent->id());
     }
 }
 
@@ -1599,7 +1599,7 @@ bool SimControl::run_entities() {
 
     // Check if any entity has NaN in its state
     for (EntityPtr &ent : ents_) {
-        if (ent->state()->pos().hasNaN()) {
+        if (ent->state_truth()->pos().hasNaN()) {
             cout << "WARNING: Entity with motion model, "
                  << ent->motion()->name() << ", contains a NaN value." << endl
                  << "Check your time step values and for NaN values coming "