adds luenberger observer system + unit test.

drake/systems/CMakeLists.txt

@@ -1,7 +1,8 @@
 add_subdirectory(analysis)
+add_subdirectory(controllers)
+add_subdirectory(estimators)
 add_subdirectory(framework)
 add_subdirectory(lcm)
 add_subdirectory(plants)
-add_subdirectory(controllers)
 add_subdirectory(robotInterfaces)
 add_subdirectory(sensors)

drake/systems/estimators/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+add_library_with_exports(LIB_NAME drakeEstimators SOURCE_FILES luenberger_observer.cc)
+target_link_libraries(drakeEstimators drakeSystemFramework)
+
+drake_install_libraries(drakeEstimators)
+drake_install_headers(luenberger_observer.h)
+drake_install_pkg_config_file(drake-estimators
+    TARGET drakeEstimators
+    LIBS -ldrakeEstimators)
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()

drake/systems/estimators/estimators.h

@@ -0,0 +1,6 @@
+
+/// @defgroup estimator_systems Estimators
+/// @{
+///   @ingroup systems
+/// @}
+

drake/systems/estimators/luenberger_observer.cc

@@ -0,0 +1,112 @@
+#include "drake/systems/estimators/luenberger_observer.h"
+
+#include <cmath>
+
+namespace drake {
+namespace systems {
+namespace estimators {
+
+template <typename T>
+LuenbergerObserver<T>::LuenbergerObserver(
+    std::unique_ptr<systems::System<T>> observed_system,
+    std::unique_ptr<systems::Context<T>> observed_system_context,
+    const Eigen::Ref<const Eigen::MatrixXd>& observer_gain)
+    : observed_system_(std::move(observed_system)),
+      observer_gain_(observer_gain),
+      observed_system_context_(std::move(observed_system_context)),
+      observed_system_output_(
+          observed_system_->AllocateOutput(*observed_system_context_)) {
+  DRAKE_DEMAND(observed_system_ != nullptr);
+
+  // Note: Could potentially extend this to MIMO systems.
+  DRAKE_DEMAND(observed_system_->get_num_input_ports() <= 1);
+  DRAKE_DEMAND(observed_system_->get_num_output_ports() == 1);
+
+  // Check that the system is a continuous-time system (only).
+  // TODO(russt): Move this functionality to the System interface (and
+  // generalize it).
+  auto x = observed_system_context_->get_continuous_state();
+  DRAKE_DEMAND(x->size() > 0);  // Otherwise there is nothing to estimate.
+  // TODO(russt/david-german): Need the methods below to exist.
+  // DRAKE_DEMAND(observed_system_context_->get_num_difference_states() == 0);
+  // DRAKE_DEMAND(observed_system_context_->get_num_modal_states() == 0);
+
+  // TODO(russt): Add support for discrete-time systems (should be easy enough
+  // to support both discrete and continuous simultaneously).
+
+  // Observer state is the (estimated) state of the observed system.
+  this->DeclareContinuousState(x->size());
+  // Output port is the (estimated) state of the observed system.
+  this->DeclareOutputPort(systems::kVectorValued, x->size(),
+                          systems::kContinuousSampling);
+  // TODO(russt): Could overload AllocateContinuousState and AllocateOutput to
+  // call AllocateContinuousState on the observed system so that I can use the
+  // actual derived class types.
+
+  // First input port is the output of the observed system.
+  this->DeclareInputPort(systems::kVectorValued,
+                         observed_system_->get_output_port(0).get_size(),
+                         systems::kContinuousSampling);
+
+  // Check the size of the gain matrix.
+  DRAKE_DEMAND(observer_gain_.rows() == x->size());
+  DRAKE_DEMAND(observer_gain_.cols() ==
+               observed_system_->get_output_port(0).get_size());
+
+  // Second input port is the input to the observed system (if it exists).
+  if (observed_system_->get_num_input_ports() > 0) {
+    this->DeclareInputPort(systems::kVectorValued,
+                           observed_system_->get_input_port(0).get_size(),
+                           systems::kContinuousSampling);
+  }
+}
+
+template <typename T>
+void LuenbergerObserver<T>::EvalOutput(const systems::Context<T>& context,
+                                       systems::SystemOutput<T>* output) const {
+  output->GetMutableVectorData(0)->set_value(
+      context.get_continuous_state_vector().CopyToVector());
+}
+
+template <typename T>
+void LuenbergerObserver<T>::EvalTimeDerivatives(
+    const systems::Context<T>& context,
+    systems::ContinuousState<T>* derivatives) const {
+  // (Note that all relevant data in the observed_system_context is set here
+  // from the observer state/inputs.  The observer_context does not hold any
+  // hidden undeclared state).
+
+  // Set observed system input.
+  if (observed_system_->get_num_input_ports() > 0) {
+    // TODO(russt): Avoid this dynamic allocation by fixing the input port once
+    // and updating it here.
+    observed_system_context_->FixInputPort(
+        0, this->EvalVectorInput(context, 1)->CopyToVector());
+  }
+  // Set observed system state.
+  // TODO(russt): Use set_value once the (derived) types match.
+  observed_system_context_->get_mutable_continuous_state_vector()
+      ->SetFromVector(context.get_continuous_state_vector().CopyToVector());
+
+  // Evaluate the observed system.
+  observed_system_->EvalOutput(*observed_system_context_,
+                               observed_system_output_.get());
+  observed_system_->EvalTimeDerivatives(*observed_system_context_, derivatives);
+
+  // Get the measurements.
+  auto y = this->EvalVectorInput(context, 0)->CopyToVector();
+  auto yhat = observed_system_output_->GetMutableVectorData(0)->CopyToVector();
+
+  // Add in the observed gain terms.
+  auto xdothat = derivatives->get_mutable_vector();
+
+  // xdothat = f(xhat,u) + L(y-yhat).
+  xdothat->SetFromVector(xdothat->CopyToVector() + observer_gain_ * (y - yhat));
+}
+
+template class LuenbergerObserver<double>;
+template class LuenbergerObserver<AutoDiffXd>;
+
+}  // namespace estimators
+}  // namespace systems
+}  // namespace drake

drake/systems/estimators/luenberger_observer.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace systems {
+namespace estimators {
+
+/// A simple state observer for a dynamical system of the form:
+/// @f[\dot{x} = f(x,u) @f]
+/// @f[y = g(x,u) @f]
+/// the observer dynamics takes the form
+/// @f[\dot{\hat{x}} = f(\hat{x},u) + L(y - g(\hat{x},u)) @f]
+/// where @f$\hat{x}@f$ is the estimated state of the original system.
+///
+/// The output of the observer system is @f$\hat{x}@f$.
+///
+/// @ingroup estimator_systems
+template <typename T>
+class LuenbergerObserver : public systems::LeafSystem<T> {
+ public:
+  /// Constructs the oberver.
+  ///
+  /// @param observed_system  The forward model for the observer.  Currently,
+  /// this system must have a maximum of one input port and exactly one output
+  /// port.
+  /// @param observed_system_context Required because it may contain parameters
+  /// which we need to evaluate the system.
+  /// @param observer_gain A m-by-n matrix where m is the number of state
+  /// variables in @p observed_system, and n is the dimension of the output port
+  /// of @p observed_system.
+  ///
+  /// Note: Takes ownership of the unique_ptrs to the observed_system and the
+  /// observed_system_context.
+  LuenbergerObserver(
+      std::unique_ptr<systems::System<T>> observed_system,
+      std::unique_ptr<systems::Context<T>> observed_system_context,
+      const Eigen::Ref<const Eigen::MatrixXd>& observer_gain);
+
+  /// Non-copyable.
+  LuenbergerObserver(const LuenbergerObserver<T>&) = delete;
+  LuenbergerObserver& operator=(const LuenbergerObserver<T>&) = delete;
+
+  /// Advance the state estimate using forward dynamics and the observer gains.
+  void EvalTimeDerivatives(
+      const systems::Context<T>& context,
+      systems::ContinuousState<T>* derivatives) const override;
+
+  /// Outputs the estimated state.
+  void EvalOutput(const systems::Context<T>& context,
+                  systems::SystemOutput<T>* output) const override;
+
+ private:
+  const std::unique_ptr<systems::System<T>> observed_system_;
+  const Eigen::MatrixXd observer_gain_;  // Gain matrix (often called "L").
+
+  // A (mutable) context is needed to efficiently call the observed system's
+  // dynamics and output methods.  Does not add any undeclared state.  This
+  // simply avoids the need to allocate a new context on every function
+  // evaluation.
+  const std::unique_ptr<systems::Context<T>> observed_system_context_;
+  const std::unique_ptr<systems::SystemOutput<T>> observed_system_output_;
+};
+
+}  // namespace estimators
+}  // namespace systems
+}  // namespace drake

drake/systems/estimators/test/CMakeLists.txt

@@ -0,0 +1,3 @@
+
+drake_add_cc_test(luenberger_observer_test luenberger_observer_test.cc)
+target_link_libraries(luenberger_observer_test drakeEstimators)

drake/systems/estimators/test/luenberger_observer_test.cc

@@ -0,0 +1,79 @@
+#include "drake/systems/estimators/luenberger_observer.h"
+
+#include <cmath>
+#include <vector>
+
+#include "gtest/gtest.h"
+
+#include "drake/common/eigen_matrix_compare.h"
+#include "drake/common/eigen_types.h"
+#include "drake/systems/framework/primitives/linear_system.h"
+
+namespace drake {
+namespace {
+
+// Test the estimator dynamics observing a linear system.
+GTEST_TEST(TestLuenberger, ErrorDynamics) {
+  Eigen::Matrix3d A;
+  Eigen::Matrix<double, 3, 1> B;
+  Eigen::Matrix<double, 2, 3> C;
+  Eigen::Vector2d D;
+  // clang-format off
+  A << 1., 2., 3.,
+       4., 5., 6.,
+       7., 8., 9.;
+  B << 10.,
+       11.,
+       12.;
+  C << 16., 17., 18.,
+       19., 20., 21.;
+  D << 22.,
+       23.;
+  // clang-format on
+
+  Eigen::Matrix<double, 3, 2> L;
+  // clang-format off
+  L << 24., 25., 26.,
+       27., 28., 29.;
+  // clang-format on
+
+  auto plant = std::make_unique<systems::LinearSystem<double>>(A, B, C, D);
+  auto plant_context = plant->CreateDefaultContext();
+  auto observer =
+      std::make_unique<systems::estimators::LuenbergerObserver<double>>(
+          std::move(plant), std::move(plant_context), L);
+
+  auto context = observer->CreateDefaultContext();
+  auto derivatives = observer->AllocateTimeDerivatives();
+  auto output = observer->AllocateOutput(*context);
+
+  // The expected dynamics are:
+  //  xhatdot = Axhat + Bu + L(y-yhat)
+  //  y = xhat
+
+  Eigen::Vector3d xhat;
+  xhat << 1.0, 2.0, 3.0;
+  Vector1d u;
+  u << 4.0;
+
+  Eigen::Vector2d y;
+  y << 5.0, 6.0;
+
+  Eigen::Vector3d xhatdot = A * xhat + B * u + L * (y - C * xhat - D * u);
+
+  context->FixInputPort(0, y);
+  context->FixInputPort(1, u);
+  context->get_mutable_continuous_state_vector()->SetFromVector(xhat);
+
+  observer->EvalTimeDerivatives(*context, derivatives.get());
+  observer->EvalOutput(*context, output.get());
+
+  double tol = 1e-10;
+
+  EXPECT_TRUE(CompareMatrices(xhatdot, derivatives->CopyToVector(), tol));
+  EXPECT_TRUE(CompareMatrices(
+      xhat, output->GetMutableVectorData(0)->CopyToVector(), tol));
+}
+
+}  // namespace
+}  // namespace drake

drake/systems/framework/primitives/gain.h

@@ -8,8 +8,9 @@
 namespace drake {
 namespace systems {
 
-/// A gain block with input `u` and output `y = k * u` with `k` a constant.
-/// The input to this system directly feeds through to its output.
+/// An element-wise gain block with input `u` and output `y = k * u` with `k` a
+/// constant vector.  The input to this system directly feeds through to its
+/// output.
 ///
 /// This class uses Drake's `-inl.h` pattern.  When seeing linker errors from
 /// this class, please refer to http://drake.mit.edu/cxx_inl.html.

drake/systems/framework/systems.h

@@ -1,13 +1,15 @@
-/** @defgroup systems Modeling Dynamical Systems
- * @{
- * @brief Drake uses a Simulink-inspired description of dynamical systems.
- *
- * Includes basic building blocks (adders, integrators, delays, etc),
- * physics models of mechanical systems, and a growing list of sensors,
- * actuators, controllers, planners, estimators.
- *
- * All dynamical systems derive from the System base class, and must
- * explicitly  declare all State, parameters, and noise/disturbances inputs.
- * The Diagram class permits modeling complex systems from libraries of parts.
- * @}
- */
+
+/// @defgroup systems Modeling Dynamical Systems
+/// @{
+/// @brief Drake uses a Simulink-inspired description of dynamical systems.
+///
+/// Includes basic building blocks (adders, integrators, delays, etc),
+/// physics models of mechanical systems, and a growing list of sensors,
+/// actuators, controllers, planners, estimators.
+///
+/// All dynamical systems derive from the drake::systems::System base class, and
+/// must explicitly declare all drake::systems::State,
+/// drake::systems::Parameters, and noise/disturbances inputs. The
+/// drake::systems::Diagram class permits modeling complex systems from
+/// libraries of parts.
+/// @}

drake/systems/sensors/rotary_encoders.h

@@ -58,9 +58,9 @@ class RotaryEncoders : public systems::LeafSystem<T> {
       const systems::Context<T>& context) const;
 
  private:
-  const int num_encoders_{0};       /// Dimension of the output port.
-  const std::vector<int> indices_;  /// Selects from the input port.
-  const std::vector<int> ticks_per_revolution_;  /// For quantization.
+  const int num_encoders_{0};       // Dimension of the output port.
+  const std::vector<int> indices_;  // Selects from the input port.
+  const std::vector<int> ticks_per_revolution_;  // For quantization.
 };
 
 }  // namespace sensors

drake/systems/sensors/sensors.h

@@ -1,6 +1,6 @@
 
-/** @defgroup sensor_systems Sensor Systems
- * @{
- * @ingroup systems
- * @}
- */
+/// @defgroup sensor_systems Sensors
+/// @{
+///   @ingroup systems
+/// @}
+