Skip to content

Commit

Permalink
[systems] LuenbergerObserver and KalmanFilter use shared_ptr (#22351)
Browse files Browse the repository at this point in the history 
The LuenbergerObserver and KalmanFilter now take the observed system
as a shared_ptr-to-const, not unique_ptr-to-mutable. The functions
don't need to mutate anything and don't need exclusive ownership, and
passing by unique_ptr causes trouble in our pydrake bindings.

Overload SteadyStateKalmanFilter to accept the context by-const-ref
instead of the unnecessary by-mutable-unique_ptr.
  • Loading branch information
@jwnimmer-tri
jwnimmer-tri authored Jan 2, 2025
1 parent 9ada504 commit 63bdcb7
Show file tree
Hide file tree
Showing 7 changed files with 121 additions and 87 deletions.
67 changes: 46 additions & 21 deletions bindings/pydrake/systems/estimators_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,9 @@ PYBIND11_MODULE(estimators, m) {
.def(py::init<const System<double>&, const Context<double>&,
const Eigen::Ref<const Eigen::MatrixXd>&>(),
py::arg("observed_system"), py::arg("observed_system_context"),
py::arg("observer_gain"), cls_doc.ctor.doc)
py::arg("observer_gain"),
// Keep alive, reference: `self` keeps `observed_system` alive.
py::keep_alive<1, 2>(), cls_doc.ctor.doc)
.def("get_observed_system_input_input_port",
&Class::get_observed_system_input_input_port,
py_rvp::reference_internal,
Expand All @@ -42,28 +44,51 @@ PYBIND11_MODULE(estimators, m) {
.def("L", &Class::L, py_rvp::reference_internal, cls_doc.L.doc);
}

m.def("SteadyStateKalmanFilter",
py::overload_cast<const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&>(&SteadyStateKalmanFilter),
py::arg("A"), py::arg("C"), py::arg("W"), py::arg("V"),
doc.SteadyStateKalmanFilter.doc_ACWV);
{
using drake::systems::LinearSystem;

m.def("SteadyStateKalmanFilter",
py::overload_cast<const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&>(&SteadyStateKalmanFilter),
py::arg("A"), py::arg("C"), py::arg("W"), py::arg("V"),
doc.SteadyStateKalmanFilter.doc_ACWV);

m.def("SteadyStateKalmanFilter",
py::overload_cast<std::unique_ptr<systems::LinearSystem<double>>,
const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&>(&SteadyStateKalmanFilter),
py::arg("system"), py::arg("W"), py::arg("V"),
doc.SteadyStateKalmanFilter.doc_linear_system);
m.def(
"SteadyStateKalmanFilter",
[](const LinearSystem<double>& system,
const Eigen::Ref<const Eigen::MatrixXd>& W,
const Eigen::Ref<const Eigen::MatrixXd>& V) {
return SteadyStateKalmanFilter(
// The lifetime of `system` is managed by the keep_alive below,
// not the C++ shared_ptr.
std::shared_ptr<const LinearSystem<double>>(
/* managed object = */ std::shared_ptr<void>{},
/* stored pointer = */ &system),
W, V);
},
py::arg("system"), py::arg("W"), py::arg("V"),
// Keep alive, reference: `result` keeps `system` alive.
py::keep_alive<0, 1>(), doc.SteadyStateKalmanFilter.doc_linear_system);

m.def("SteadyStateKalmanFilter",
py::overload_cast<std::unique_ptr<System<double>>,
std::unique_ptr<Context<double>>,
const Eigen::Ref<const Eigen::MatrixXd>&,
const Eigen::Ref<const Eigen::MatrixXd>&>(&SteadyStateKalmanFilter),
py::arg("system"), py::arg("context"), py::arg("W"), py::arg("V"),
doc.SteadyStateKalmanFilter.doc_system);
m.def(
"SteadyStateKalmanFilter",
[](const System<double>& system, const Context<double>& context,
const Eigen::Ref<const Eigen::MatrixXd>& W,
const Eigen::Ref<const Eigen::MatrixXd>& V) {
return SteadyStateKalmanFilter(
// The lifetime of `system` is managed by the keep_alive below,
// not the C++ shared_ptr.
std::shared_ptr<const System<double>>(
/* managed object = */ std::shared_ptr<void>{},
/* stored pointer = */ &system),
context, W, V);
},
py::arg("system"), py::arg("context"), py::arg("W"), py::arg("V"),
// Keep alive, reference: `result` keeps `system` alive.
py::keep_alive<0, 1>(), doc.SteadyStateKalmanFilter.doc_system);
}
}

} // namespace pydrake
Expand Down
24 changes: 18 additions & 6 deletions systems/estimators/kalman_filter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ Eigen::MatrixXd SteadyStateKalmanFilter(
}

std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
std::unique_ptr<LinearSystem<double>> system,
std::shared_ptr<const LinearSystem<double>> system,
const Eigen::Ref<const Eigen::MatrixXd>& W,
const Eigen::Ref<const Eigen::MatrixXd>& V) {
const Eigen::MatrixXd L =
Expand All @@ -34,25 +34,37 @@ std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
}

std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
std::unique_ptr<System<double>> system,
std::unique_ptr<Context<double>> context,
std::shared_ptr<const System<double>> system,
const Context<double>& context,
const Eigen::Ref<const Eigen::MatrixXd>& W,
const Eigen::Ref<const Eigen::MatrixXd>& V) {
DRAKE_DEMAND(context->get_continuous_state_vector().size() >
DRAKE_DEMAND(context.get_continuous_state_vector().size() >
0); // Otherwise, I don't need an estimator.
DRAKE_DEMAND(system->num_output_ports() ==
1); // Need measurements to estimate state.

// TODO(russt): Demand time-invariant once we can.
// TODO(russt): Check continuous-time (only).

auto linear_system = Linearize(*system, *context);
std::unique_ptr<LinearSystem<double>> linear_system =
Linearize(*system, context);

const Eigen::MatrixXd L =
SteadyStateKalmanFilter(linear_system->A(), linear_system->C(), W, V);

return std::make_unique<LuenbergerObserver<double>>(std::move(system),
*context, L);
context, L);
}

// N.B. This is the to-be-deprecated overload.
std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
std::unique_ptr<System<double>> system,
std::unique_ptr<Context<double>> context,
const Eigen::Ref<const Eigen::MatrixXd>& W,
const Eigen::Ref<const Eigen::MatrixXd>& V) {
DRAKE_THROW_UNLESS(context != nullptr);
return SteadyStateKalmanFilter(
std::shared_ptr<const System<double>>(std::move(system)), *context, W, V);
}

} // namespace estimators
Expand Down
33 changes: 18 additions & 15 deletions systems/estimators/kalman_filter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,19 +44,17 @@ Eigen::MatrixXd SteadyStateKalmanFilter(
/// Creates a Luenberger observer system using the optimal steady-state Kalman
/// filter gain matrix, L, as described above.
///
/// @param system A unique_ptr to a LinearSystem describing the system to be
/// observed. The new observer will take and maintain ownership of this
/// pointer.
/// @param system The LinearSystem describing the system to be observed.
/// @param W The process noise covariance matrix, E[ww'], of size num_states x
/// num_states.
/// @param V The measurement noise covariance matrix, E[vv'], of size num_.
/// @returns A unique_ptr to the constructed observer system.
/// @returns The constructed observer system.
///
/// @throws std::exception if V is not positive definite.
/// @ingroup estimator_systems
/// @pydrake_mkdoc_identifier{linear_system}
std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
std::unique_ptr<LinearSystem<double>> system,
std::shared_ptr<const LinearSystem<double>> system,
const Eigen::Ref<const Eigen::MatrixXd>& W,
const Eigen::Ref<const Eigen::MatrixXd>& V);

Expand All @@ -70,24 +68,29 @@ std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
/// then the resulting observer will have the form
/// dx̂/dt = f(x̂,u) + L(y - g(x̂,u)),
/// where x̂ is the estimated state and the gain matrix, L, is designed
/// as a steady-state Kalman filter using a linearization of f(x,u) at @p
/// context as described above.
/// as a steady-state Kalman filter using a linearization of f(x,u) at
/// `context` as described above.
///
/// @param system A unique_ptr to a System describing the system to be
/// observed. The new observer will take and maintain ownership of this
/// pointer.
/// @param context A unique_ptr to the context describing a fixed-point of the
/// system (plus any additional parameters). The new observer will take and
/// maintain ownership of this pointer for use in its internal forward
/// simulation.
/// @param system The System describing the system to be observed.
/// @param context The context describing a fixed-point of the system (plus any
/// additional parameters).
/// @param W The process noise covariance matrix, E[ww'], of size num_states x
/// num_states.
/// @param V The measurement noise covariance matrix, E[vv'], of size num_.
/// @returns A unique_ptr to the constructed observer system.
/// @returns The constructed observer system.
///
/// @throws std::exception if V is not positive definite.
/// @ingroup estimator_systems
/// @pydrake_mkdoc_identifier{system}
std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
std::shared_ptr<const System<double>> system,
const Context<double>& context,
const Eigen::Ref<const Eigen::MatrixXd>& W,
const Eigen::Ref<const Eigen::MatrixXd>& V);

// TODO(jwnimmer-tri) Add deprecation marker on or about 2025-02-01.
/// (To be deprecated) An overload that accepts `context` by unique_ptr.
/// @exclude_from_pydrake_mkdoc{This is not bound.}
std::unique_ptr<LuenbergerObserver<double>> SteadyStateKalmanFilter(
std::unique_ptr<System<double>> system,
std::unique_ptr<Context<double>> context,
Expand Down
19 changes: 6 additions & 13 deletions systems/estimators/luenberger_observer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,12 @@ namespace estimators {

template <typename T>
LuenbergerObserver<T>::LuenbergerObserver(
const System<T>* system, std::unique_ptr<System<T>> owned_system,
std::shared_ptr<const System<T>> observed_system,
const Context<T>& observed_system_context,
const Eigen::Ref<const Eigen::MatrixXd>& observer_gain)
: owned_system_(std::move(owned_system)),
observed_system_(owned_system_ ? owned_system_.get() : system),
: observed_system_(std::move(observed_system)),
observer_gain_(observer_gain) {
DRAKE_DEMAND(observed_system_ != nullptr);
DRAKE_THROW_UNLESS(observed_system_ != nullptr);
observed_system_->ValidateContext(observed_system_context);

// Note: Could potentially extend this to MIMO systems.
Expand Down Expand Up @@ -83,15 +82,9 @@ template <typename T>
LuenbergerObserver<T>::LuenbergerObserver(
const System<T>& observed_system, const Context<T>& observed_system_context,
const Eigen::Ref<const Eigen::MatrixXd>& observer_gain)
: LuenbergerObserver(&observed_system, nullptr, observed_system_context,
observer_gain) {}

template <typename T>
LuenbergerObserver<T>::LuenbergerObserver(
std::unique_ptr<System<T>> observed_system,
const Context<T>& observed_system_context,
const Eigen::Ref<const Eigen::MatrixXd>& observer_gain)
: LuenbergerObserver(nullptr, std::move(observed_system),
: LuenbergerObserver(std::shared_ptr<const System<T>>(
/* managed object = */ std::shared_ptr<void>{},
/* stored pointer = */ &observed_system),
observed_system_context, observer_gain) {}

template <typename T>
Expand Down
21 changes: 6 additions & 15 deletions systems/estimators/luenberger_observer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,16 +50,15 @@ class LuenbergerObserver final: public LeafSystem<T> {
/// of @p observed_system.
///
/// @pre The observed_system output port must be vector-valued.
///
/// Note: The `observed_system` reference must remain valid for the lifetime
/// of this system.
LuenbergerObserver(const System<T>& observed_system,
LuenbergerObserver(std::shared_ptr<const System<T>> observed_system,
const Context<T>& observed_system_context,
const Eigen::Ref<const Eigen::MatrixXd>& observer_gain);

/// Constructs the observer, taking ownership of `observed_system`.
/// Constructs the observer, without claiming ownership of `observed_system`.
/// Note: The `observed_system` reference must remain valid for the lifetime
/// of this system.
/// @exclude_from_pydrake_mkdoc{This constructor is not bound.}
LuenbergerObserver(std::unique_ptr<System<T>> observed_system,
LuenbergerObserver(const System<T>& observed_system,
const Context<T>& observed_system_context,
const Eigen::Ref<const Eigen::MatrixXd>& observer_gain);

Expand Down Expand Up @@ -88,13 +87,6 @@ class LuenbergerObserver final: public LeafSystem<T> {
const Eigen::MatrixXd& L() { return observer_gain_; }

private:
// All constructors delegate here. Exactly one of system or owned_system must
// be non-null.
LuenbergerObserver(const System<T>* system,
std::unique_ptr<System<T>> owned_system,
const Context<T>& context,
const Eigen::Ref<const Eigen::MatrixXd>& observer_gain);

// Advance the state estimate using forward dynamics and the observer
// gains.
void DoCalcTimeDerivatives(const Context<T>& context,
Expand All @@ -107,8 +99,7 @@ class LuenbergerObserver final: public LeafSystem<T> {
void UpdateObservedSystemContext(const Context<T>& context,
Context<T>* observed_system_context) const;

const std::unique_ptr<System<T>> owned_system_{};
const System<T>* const observed_system_;
const std::shared_ptr<const System<T>> observed_system_;
const Eigen::MatrixXd observer_gain_; // Gain matrix (often called "L").

const CacheEntry* observed_system_context_cache_entry_{};
Expand Down
17 changes: 11 additions & 6 deletions systems/estimators/test/kalman_filter_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,20 +38,25 @@ GTEST_TEST(TestKalman, DoubleIntegrator) {
EXPECT_TRUE(CompareMatrices(SteadyStateKalmanFilter(A, C, W, V), L, tol));

// Test LinearSystem version of the Kalman filter.
auto linear_filter = SteadyStateKalmanFilter(
std::make_unique<LinearSystem<double>>(A, B, C, D), W, V);

auto sys = std::make_shared<const LinearSystem<double>>(A, B, C, D);
auto linear_filter = SteadyStateKalmanFilter(sys, W, V);
EXPECT_TRUE(CompareMatrices(linear_filter->L(), L, tol));

// Call it as a generic System (by passing in a Context).
// Should get the same result, but as an affine system.
auto sys = std::make_unique<LinearSystem<double>>(A, B, C, D);
auto context = sys->CreateDefaultContext();
sys->get_input_port().FixValue(context.get(), 0.0);
context->SetContinuousState(Eigen::Vector2d::Zero());
auto filter =
SteadyStateKalmanFilter(std::move(sys), std::move(context), W, V);
auto filter = SteadyStateKalmanFilter(sys, *context, W, V);
context.reset();
EXPECT_TRUE(CompareMatrices(filter->L(), L, tol));

// Also check the to-be-deprecated overload that accepts a unique_ptr context.
auto owned = std::make_unique<LinearSystem<double>>(A, B, C, D);
context = owned->CreateDefaultContext();
owned->get_input_port().FixValue(context.get(), 0.0);
context->SetContinuousState(Eigen::Vector2d::Zero());
filter = SteadyStateKalmanFilter(std::move(owned), std::move(context), W, V);
EXPECT_TRUE(CompareMatrices(filter->L(), L, tol));
}

Expand Down
27 changes: 16 additions & 11 deletions systems/estimators/test/luenberger_observer_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,22 +44,27 @@ GTEST_TEST(LuenbergerObserverTest, ErrorDynamics) {

// Run the test using both constructors.
for (int i = 0; i < 2; ++i) {
auto plant = std::make_unique<systems::LinearSystem<double>>(A, B, C, D);
auto plant = std::make_shared<const LinearSystem<double>>(A, B, C, D);
auto plant_context = plant->CreateDefaultContext();
std::unique_ptr<systems::estimators::LuenbergerObserver<double>> observer{
nullptr};
std::unique_ptr<LuenbergerObserver<double>> observer{nullptr};
switch (i) {
case 0: // pass system and context by reference.
observer =
std::make_unique<systems::estimators::LuenbergerObserver<double>>(
*plant, *plant_context, L);
case 0: {
// Construct using the shared_ptr overload. We move the plant into the
// constructor so that the constructor argument is the only thing
// keeping it alive.
observer = std::make_unique<LuenbergerObserver<double>>(
std::move(plant), *plant_context, L);
break;
case 1: // owned system version.
observer =
std::make_unique<systems::estimators::LuenbergerObserver<double>>(
std::move(plant), *plant_context, L);
}
case 1: {
// Construct using the const-ref overload. The local variable `plant`
// must remain intact because the LuenbergerObserver still aliases it.
observer = std::make_unique<LuenbergerObserver<double>>(
*plant, *plant_context, L);
break;
}
}
plant_context.reset();

auto context = observer->CreateDefaultContext();
auto derivatives = observer->AllocateTimeDerivatives();
Expand Down

0 comments on commit 63bdcb7

Please sign in to comment.