pydrake.systems.estimators
- pydrake.systems.estimators.DiscreteTimeSteadyStateKalmanFilter(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], C: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], W: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], V: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.float64[m, n]]
- Computes the optimal observer gain, L, for the discrete-time linear system defined by \[x[n+1] = Ax[n] + Bu[n] + w,\]\[y[n] = Cx[n] + Du[n] + v.\]- The resulting observer is of the form \[\hat{x}[n+1] = A\hat{x}[n] + Bu[n] + L(y - C\hat{x}[n] - Du[n]).\]- The process noise, w, and the measurement noise, v, are assumed to be iid mean-zero Gaussian. - This is a simplified form of the full Kalman filter obtained by assuming that the state-covariance matrix has already converged to its steady-state solution P, and the observer gain L = APC’(CPC’ + V)⁻¹. - Parameter A:
- The state-space dynamics matrix of size num_states x num_states. 
- Parameter C:
- The state-space output matrix of size num_outputs x num_states. 
- Parameter W:
- The process noise covariance matrix, E[ww’], of size num_states x num_states. 
- Parameter V:
- The measurement noise covariance matrix, E[vv’], of size num_outputs x num_outputs. 
 - Returns:
- The steady-state observer gain matrix of size num_states x num_outputs. 
- Raises:
- RuntimeError if W is not positive semi-definite or if V is not – 
- positive definite. – 
 
 
- Parameter 
- class pydrake.systems.estimators.LuenbergerObserver
- Bases: - pydrake.systems.framework.LeafSystem- A simple state observer for a continuous-time dynamical system of the form: \[\dot{x} = f(x,u)\]\[y = g(x,u)\]- the observer dynamics takes the form \[\dot{\hat{x}} = f(\hat{x},u) + L(y - g(\hat{x},u))\]- where \(\hat{x}\) is the estimated state of the original system. The output of the observer system is \(\hat{x}\). - Or a simple state observer for a discrete-time dynamical system of the form: \[x[n+1] = f_d(x[n],u[n])\]\[y[n] = g(x[n],u[n])\]- the observer dynamics takes the form \[\hat{x}[n+1] = f_d(\hat{x}[n],u[n]) + L(y - g(\hat{x}[n],u[n]))\]- where \(\hat{x}\) is the estimated state of the original system. The output of the observer system is \(\hat{x}[n+1]\). - observed_system_input→ - observed_system_output→ - LuenbergerObserver - → estimated_state - Note - This class is templated; see - LuenbergerObserver_for the list of instantiations.- __init__(self: pydrake.systems.estimators.LuenbergerObserver, observed_system: pydrake.systems.framework.System, observed_system_context: pydrake.systems.framework.Context, observer_gain: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) None
- Constructs the observer. - Parameter observed_system:
- The forward model for the observer. Currently, this system must have a maximum of one input port and exactly one output port. 
- Parameter observed_system_context:
- Required because it may contain parameters which we need to evaluate the system. 
- Parameter observer_gain:
- A m-by-n matrix where m is the number of state variables in - observed_system, and n is the dimension of the output port of- observed_system.
- Precondition:
- The observed_system output port must be vector-valued. 
 
- Parameter 
 - get_estimated_state_output_port(self: pydrake.systems.estimators.LuenbergerObserver) pydrake.systems.framework.OutputPort
 - get_observed_system_input_input_port(self: pydrake.systems.estimators.LuenbergerObserver) pydrake.systems.framework.InputPort
 - get_observed_system_output_input_port(self: pydrake.systems.estimators.LuenbergerObserver) pydrake.systems.framework.InputPort
 - L(self: pydrake.systems.estimators.LuenbergerObserver) numpy.ndarray[numpy.float64[m, n]]
- Provides access via the short-hand name, L, too. 
 - observer_gain(self: pydrake.systems.estimators.LuenbergerObserver) numpy.ndarray[numpy.float64[m, n]]
- Provides access to the observer gain. 
 
- template pydrake.systems.estimators.LuenbergerObserver_
- Instantiations: - LuenbergerObserver_[float],- LuenbergerObserver_[AutoDiffXd],- LuenbergerObserver_[Expression]
- class pydrake.systems.estimators.LuenbergerObserver_[AutoDiffXd]
- Bases: - pydrake.systems.framework.LeafSystem_[AutoDiffXd]- A simple state observer for a continuous-time dynamical system of the form: \[\dot{x} = f(x,u)\]\[y = g(x,u)\]- the observer dynamics takes the form \[\dot{\hat{x}} = f(\hat{x},u) + L(y - g(\hat{x},u))\]- where \(\hat{x}\) is the estimated state of the original system. The output of the observer system is \(\hat{x}\). - Or a simple state observer for a discrete-time dynamical system of the form: \[x[n+1] = f_d(x[n],u[n])\]\[y[n] = g(x[n],u[n])\]- the observer dynamics takes the form \[\hat{x}[n+1] = f_d(\hat{x}[n],u[n]) + L(y - g(\hat{x}[n],u[n]))\]- where \(\hat{x}\) is the estimated state of the original system. The output of the observer system is \(\hat{x}[n+1]\). - observed_system_input→ - observed_system_output→ - LuenbergerObserver - → estimated_state - __init__(self: pydrake.systems.estimators.LuenbergerObserver_[AutoDiffXd], observed_system: pydrake.systems.framework.System_[AutoDiffXd], observed_system_context: pydrake.systems.framework.Context_[AutoDiffXd], observer_gain: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) None
- Constructs the observer. - Parameter observed_system:
- The forward model for the observer. Currently, this system must have a maximum of one input port and exactly one output port. 
- Parameter observed_system_context:
- Required because it may contain parameters which we need to evaluate the system. 
- Parameter observer_gain:
- A m-by-n matrix where m is the number of state variables in - observed_system, and n is the dimension of the output port of- observed_system.
- Precondition:
- The observed_system output port must be vector-valued. 
 
- Parameter 
 - get_estimated_state_output_port(self: pydrake.systems.estimators.LuenbergerObserver_[AutoDiffXd]) pydrake.systems.framework.OutputPort_[AutoDiffXd]
 - get_observed_system_input_input_port(self: pydrake.systems.estimators.LuenbergerObserver_[AutoDiffXd]) pydrake.systems.framework.InputPort_[AutoDiffXd]
 - get_observed_system_output_input_port(self: pydrake.systems.estimators.LuenbergerObserver_[AutoDiffXd]) pydrake.systems.framework.InputPort_[AutoDiffXd]
 - L(self: pydrake.systems.estimators.LuenbergerObserver_[AutoDiffXd]) numpy.ndarray[numpy.float64[m, n]]
- Provides access via the short-hand name, L, too. 
 - observer_gain(self: pydrake.systems.estimators.LuenbergerObserver_[AutoDiffXd]) numpy.ndarray[numpy.float64[m, n]]
- Provides access to the observer gain. 
 
- class pydrake.systems.estimators.LuenbergerObserver_[Expression]
- Bases: - pydrake.systems.framework.LeafSystem_[Expression]- A simple state observer for a continuous-time dynamical system of the form: \[\dot{x} = f(x,u)\]\[y = g(x,u)\]- the observer dynamics takes the form \[\dot{\hat{x}} = f(\hat{x},u) + L(y - g(\hat{x},u))\]- where \(\hat{x}\) is the estimated state of the original system. The output of the observer system is \(\hat{x}\). - Or a simple state observer for a discrete-time dynamical system of the form: \[x[n+1] = f_d(x[n],u[n])\]\[y[n] = g(x[n],u[n])\]- the observer dynamics takes the form \[\hat{x}[n+1] = f_d(\hat{x}[n],u[n]) + L(y - g(\hat{x}[n],u[n]))\]- where \(\hat{x}\) is the estimated state of the original system. The output of the observer system is \(\hat{x}[n+1]\). - observed_system_input→ - observed_system_output→ - LuenbergerObserver - → estimated_state - __init__(self: pydrake.systems.estimators.LuenbergerObserver_[Expression], observed_system: pydrake.systems.framework.System_[Expression], observed_system_context: pydrake.systems.framework.Context_[Expression], observer_gain: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) None
- Constructs the observer. - Parameter observed_system:
- The forward model for the observer. Currently, this system must have a maximum of one input port and exactly one output port. 
- Parameter observed_system_context:
- Required because it may contain parameters which we need to evaluate the system. 
- Parameter observer_gain:
- A m-by-n matrix where m is the number of state variables in - observed_system, and n is the dimension of the output port of- observed_system.
- Precondition:
- The observed_system output port must be vector-valued. 
 
- Parameter 
 - get_estimated_state_output_port(self: pydrake.systems.estimators.LuenbergerObserver_[Expression]) pydrake.systems.framework.OutputPort_[Expression]
 - get_observed_system_input_input_port(self: pydrake.systems.estimators.LuenbergerObserver_[Expression]) pydrake.systems.framework.InputPort_[Expression]
 - get_observed_system_output_input_port(self: pydrake.systems.estimators.LuenbergerObserver_[Expression]) pydrake.systems.framework.InputPort_[Expression]
 - L(self: pydrake.systems.estimators.LuenbergerObserver_[Expression]) numpy.ndarray[numpy.float64[m, n]]
- Provides access via the short-hand name, L, too. 
 - observer_gain(self: pydrake.systems.estimators.LuenbergerObserver_[Expression]) numpy.ndarray[numpy.float64[m, n]]
- Provides access to the observer gain. 
 
- pydrake.systems.estimators.SteadyStateKalmanFilter(*args, **kwargs)
- Overloaded function. - SteadyStateKalmanFilter(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], C: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], W: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], V: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> numpy.ndarray[numpy.float64[m, n]] 
 - Computes the optimal observer gain, L, for the continuous-time linear system defined by \[\dot{x} = Ax + Bu + w,\]\[y = Cx + Du + v.\]- The resulting observer is of the form \[\dot{\hat{x}} = A\hat{x} + Bu + L(y - C\hat{x} - Du).\]- The process noise, w, and the measurement noise, v, are assumed to be iid mean-zero Gaussian. - This is a simplified form of the full Kalman filter obtained by assuming that the state-covariance matrix has already converged to its steady-state solution. - Parameter A:
- The state-space dynamics matrix of size num_states x num_states. 
- Parameter C:
- The state-space output matrix of size num_outputs x num_states. 
- Parameter W:
- The process noise covariance matrix, E[ww’], of size num_states x num_states. 
- Parameter V:
- The measurement noise covariance matrix, E[vv’], of size num_outputs x num_outputs. 
 - Returns:
- The steady-state observer gain matrix of size num_states x num_outputs. 
- Raises:
- RuntimeError if V is not positive definite. – 
 - SteadyStateKalmanFilter(system: pydrake.systems.primitives.LinearSystem, W: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], V: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> pydrake.systems.estimators.LuenbergerObserver 
 - Creates a Luenberger observer system using the optimal steady-state Kalman filter gain matrix, L, as described in SteadyStateKalmanFilter and DiscreteTimeSteadyStateKalmanFilter. - Parameter system:
- The LinearSystem describing the system to be observed. 
- Parameter W:
- The process noise covariance matrix, E[ww’], of size num_states x num_states. 
- Parameter V:
- The measurement noise covariance matrix, E[vv’], of size num_outputs x num_outputs. 
 - Returns:
- The constructed observer system. 
- Raises:
- RuntimeError if V is not positive definite. – 
 - SteadyStateKalmanFilter(system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, W: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], V: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> pydrake.systems.estimators.LuenbergerObserver 
 - Creates a Luenberger observer system using the steady-state Kalman filter observer gain. - If - systemhas continuous-time dynamics: ẋ = f(x,u), and the output: y = g(x,u), 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- contextas described above.- If - systemhas discrete-time dynamics: x[n+1] = f(x[n],u[n]), and the output: y[n] = g(x[n],u[n]), then the resulting observer will have the form x̂[n+1] = f(x̂[n],u[n]) + L(y − g(x̂[n],u[n])), where x̂[n+1] 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- contextas described above.- Parameter system:
- The System describing the system to be observed. 
- Parameter context:
- The context describing a fixed-point of the system (plus any additional parameters). 
- Parameter W:
- The process noise covariance matrix, E[ww’], of size num_states x num_states. 
- Parameter V:
- The measurement noise covariance matrix, E[vv’], of size num_outputs x num_outputs. 
 - Returns:
- The constructed observer system. 
- Raises:
- RuntimeError if V is not positive definite. –