Robust Hybrid Optimal Controller. More...

#include <RobustDDPController.hpp>

Collaboration diagram for fsw::control::RobustDDPController:

Classes
struct	Config
struct	Measurement
struct	NavState
struct	Command
struct	ActuationCommand
struct	EngineEstimate
struct	Diagnostics
struct	ControllerState
struct	Reference
struct	DDPSolution

Public Types
enum class	CommandType { THRUST_DESIRED , ALTITUDE_GOAL , PRESSURE_TARGET }

Public Member Functions
	RobustDDPController ()
	~RobustDDPController ()
bool	initialize (const Config &config)
void	reset ()
std::pair< ActuationCommand, Diagnostics >	step (const Measurement &meas, const NavState &nav, const Command &cmd)
	Execute one control step.
ControllerState	getState () const

Private Member Functions
Eigen::VectorXd	dynamicsStep (const Eigen::VectorXd &x, const Eigen::VectorXd &u, double mdot_F, double mdot_O) const
void	linearize (const Eigen::VectorXd &x, const Eigen::VectorXd &u, double mdot_F, double mdot_O, Eigen::MatrixXd &A, Eigen::MatrixXd &B) const
EngineEstimate	estimateEngine (double P_d_fuel, double P_d_ox) const
Reference	buildReference (const NavState &nav, const Measurement &meas, const Command &cmd) const
Eigen::VectorXd	buildState (const Measurement &meas) const
bool	isStateSafe (const Eigen::VectorXd &x, CommandType cmd_type=CommandType::THRUST_DESIRED) const
DDPSolution	solveDDP (const Eigen::VectorXd &x0, const Reference &ref, const Eigen::MatrixXd &u_init) const
double	runningCost (const Eigen::VectorXd &x, const Eigen::VectorXd &u, const Reference &ref, int k, const Eigen::VectorXd &u_prev) const
Eigen::VectorXd	enumerativeMPC (const Eigen::VectorXd &x0, const Reference &ref, int k) const
Eigen::VectorXd	economicDecision (const Eigen::VectorXd &x, const Reference &ref, int k) const
Eigen::VectorXd	filterAction (const Eigen::VectorXd &x, const Eigen::VectorXd &u_proposed, const Reference &ref, int k) const
bool	isActionSafe (const Eigen::VectorXd &x, const Eigen::VectorXd &u, CommandType cmd_type, int num_steps=2) const
bool	shouldCutoff (const NavState &nav) const
ActuationCommand	computeActuation (const Eigen::VectorXd &u_relaxed) const

Private Attributes
Config	config_
ControllerState	state_
std::mutex	state_mutex_
std::optional< Eigen::VectorXd >	x_prev_
std::optional< Eigen::VectorXd >	u_prev_applied_
std::optional< Eigen::MatrixXd >	u_seq_prev_
int	tick_ = 0

Detailed Description

Robust Hybrid Optimal Controller.

Implements the full mathematical framework from the paper: §4 Blowdown ullage dynamics (ideal-gas law, choked/subsonic flow) §6 Injector + chamber model (Cd·A·√(2ρΔP), chamber root-find) §7 Hard combustion constraints (MR, injector stiffness) §9 Robust min-max MPC §10 Enumerative robust MPC for binary valves §11 Open/close economic decision logic §12 Supervisory cutoff

DDP-based continuous relaxation (iLQR-style)

Definition at line 53 of file RobustDDPController.hpp.

Member Enumeration Documentation

◆ CommandType

enum class fsw::control::RobustDDPController::CommandType

strong

Enumerator
THRUST_DESIRED
ALTITUDE_GOAL
PRESSURE_TARGET

Definition at line 145 of file RobustDDPController.hpp.

Constructor & Destructor Documentation

◆ RobustDDPController()

fsw::control::RobustDDPController::RobustDDPController ( )

Definition at line 17 of file RobustDDPController.cpp.

References fsw::control::N_STATE, and state_.

◆ ~RobustDDPController()

fsw::control::RobustDDPController::~RobustDDPController ( )

default

Member Function Documentation

◆ buildReference()

RobustDDPController::Reference fsw::control::RobustDDPController::buildReference	(	const NavState &	nav,
		const Measurement &	meas,
		const Command &	cmd ) const

private

Definition at line 395 of file RobustDDPController.cpp.

Referenced by step().

◆ buildState()

Eigen::VectorXd fsw::control::RobustDDPController::buildState ( const Measurement & meas ) const

private

◆ computeActuation()

RobustDDPController::ActuationCommand fsw::control::RobustDDPController::computeActuation ( const Eigen::VectorXd & u_relaxed ) const

private

Definition at line 887 of file RobustDDPController.cpp.

References config_, fsw::control::RobustDDPController::ActuationCommand::duty_F, fsw::control::RobustDDPController::ActuationCommand::duty_O, fsw::control::RobustDDPController::ActuationCommand::u_F_on, fsw::control::RobustDDPController::ActuationCommand::u_O_on, and fsw::control::RobustDDPController::ActuationCommand::valid.

Referenced by step().

◆ dynamicsStep()

Eigen::VectorXd fsw::control::RobustDDPController::dynamicsStep	(	const Eigen::VectorXd &	x,
		const Eigen::VectorXd &	u,
		double	mdot_F,
		double	mdot_O ) const

private

Definition at line 166 of file RobustDDPController.cpp.

References config_, fsw::control::GAMMA_N2, fsw::control::IDX_M_GAS_COPV, fsw::control::IDX_M_GAS_F, fsw::control::IDX_M_GAS_O, fsw::control::IDX_P_COPV, fsw::control::IDX_P_D_F, fsw::control::IDX_P_D_O, fsw::control::IDX_P_U_F, fsw::control::IDX_P_U_O, fsw::control::IDX_V_U_F, fsw::control::IDX_V_U_O, fsw::control::N_STATE, and fsw::control::R_GAS_N2.

Referenced by economicDecision(), enumerativeMPC(), isActionSafe(), linearize(), solveDDP(), and step().

◆ economicDecision()

Eigen::VectorXd fsw::control::RobustDDPController::economicDecision	(	const Eigen::VectorXd &	x,
		const Reference &	ref,
		int	k ) const

private

Definition at line 771 of file RobustDDPController.cpp.

References config_, dynamicsStep(), estimateEngine(), fsw::control::RobustDDPController::Reference::F_ref, fsw::control::IDX_P_D_F, fsw::control::IDX_P_D_O, fsw::control::N_CONTROL, fsw::control::RobustDDPController::Reference::P_fuel_ref, fsw::control::RobustDDPController::Reference::P_ox_ref, PRESSURE_TARGET, and fsw::control::RobustDDPController::Reference::type.

Referenced by step().

◆ enumerativeMPC()

Eigen::VectorXd fsw::control::RobustDDPController::enumerativeMPC	(	const Eigen::VectorXd &	x0,
		const Reference &	ref,
		int	k ) const

private

Definition at line 709 of file RobustDDPController.cpp.

References config_, dynamicsStep(), estimateEngine(), fsw::control::IDX_P_D_F, fsw::control::IDX_P_D_O, isStateSafe(), fsw::control::N_CONTROL, PRESSURE_TARGET, runningCost(), state_, step(), fsw::control::RobustDDPController::Reference::type, and u_prev_applied_.

Referenced by step().

◆ estimateEngine()

RobustDDPController::EngineEstimate fsw::control::RobustDDPController::estimateEngine	(	double	P_d_fuel,
		double	P_d_ox ) const

private

Definition at line 336 of file RobustDDPController.cpp.

References config_, fsw::control::RobustDDPController::EngineEstimate::dp_F, fsw::control::RobustDDPController::EngineEstimate::dp_O, fsw::control::RobustDDPController::EngineEstimate::F, fsw::control::RobustDDPController::EngineEstimate::mdot_F, fsw::control::RobustDDPController::EngineEstimate::mdot_O, fsw::control::RobustDDPController::EngineEstimate::MR, fsw::control::RobustDDPController::EngineEstimate::P_ch, and fsw::control::RobustDDPController::EngineEstimate::valid.

Referenced by economicDecision(), enumerativeMPC(), isActionSafe(), isStateSafe(), runningCost(), solveDDP(), and step().

◆ filterAction()

Eigen::VectorXd fsw::control::RobustDDPController::filterAction	(	const Eigen::VectorXd &	x,
		const Eigen::VectorXd &	u_proposed,
		const Reference &	ref,
		int	k ) const

private

Definition at line 842 of file RobustDDPController.cpp.

References isActionSafe(), fsw::control::N_CONTROL, runningCost(), fsw::control::RobustDDPController::Reference::type, and u_prev_applied_.

Referenced by step().

◆ getState()

RobustDDPController::ControllerState fsw::control::RobustDDPController::getState ( ) const

Definition at line 157 of file RobustDDPController.cpp.

References lock, state_, and state_mutex_.

◆ initialize()

bool fsw::control::RobustDDPController::initialize ( const Config & config )

Definition at line 26 of file RobustDDPController.cpp.

References config_, and reset().

Referenced by main().

◆ isActionSafe()

bool fsw::control::RobustDDPController::isActionSafe	(	const Eigen::VectorXd &	x,
		const Eigen::VectorXd &	u,
		CommandType	cmd_type,
		int	num_steps = 2 ) const

private

Definition at line 823 of file RobustDDPController.cpp.

References dynamicsStep(), estimateEngine(), fsw::control::IDX_P_D_F, fsw::control::IDX_P_D_O, isStateSafe(), and state_.

Referenced by filterAction().

◆ isStateSafe()

bool fsw::control::RobustDDPController::isStateSafe	(	const Eigen::VectorXd &	x,
		CommandType	cmd_type = CommandType::THRUST_DESIRED ) const

private

Definition at line 462 of file RobustDDPController.cpp.

References config_, estimateEngine(), fsw::control::IDX_P_COPV, fsw::control::IDX_P_D_F, fsw::control::IDX_P_D_O, fsw::control::IDX_P_U_F, fsw::control::IDX_P_U_O, and PRESSURE_TARGET.

Referenced by enumerativeMPC(), and isActionSafe().

◆ linearize()

void fsw::control::RobustDDPController::linearize	(	const Eigen::VectorXd &	x,
		const Eigen::VectorXd &	u,
		double	mdot_F,
		double	mdot_O,
		Eigen::MatrixXd &	A,
		Eigen::MatrixXd &	B ) const

private

Definition at line 311 of file RobustDDPController.cpp.

References dynamicsStep(), fsw::control::N_CONTROL, and fsw::control::N_STATE.

Referenced by solveDDP().

◆ reset()

void fsw::control::RobustDDPController::reset ( )

Definition at line 32 of file RobustDDPController.cpp.

References config_, lock, fsw::control::N_STATE, state_, state_mutex_, tick_, u_prev_applied_, u_seq_prev_, and x_prev_.

Referenced by initialize(), and main().

◆ runningCost()

double fsw::control::RobustDDPController::runningCost	(	const Eigen::VectorXd &	x,
		const Eigen::VectorXd &	u,
		const Reference &	ref,
		int	k,
		const Eigen::VectorXd &	u_prev ) const

private

Definition at line 496 of file RobustDDPController.cpp.

References config_, estimateEngine(), fsw::control::RobustDDPController::Reference::F_ref, fsw::control::IDX_P_COPV, fsw::control::IDX_P_D_F, fsw::control::IDX_P_D_O, fsw::control::RobustDDPController::Reference::MR_ref, fsw::control::RobustDDPController::Reference::P_fuel_ref, fsw::control::RobustDDPController::Reference::P_ox_ref, PRESSURE_TARGET, and fsw::control::RobustDDPController::Reference::type.

Referenced by enumerativeMPC(), filterAction(), solveDDP(), and step().

◆ shouldCutoff()

bool fsw::control::RobustDDPController::shouldCutoff ( const NavState & nav ) const

private

Definition at line 876 of file RobustDDPController.cpp.

References config_, and fsw::control::G0.

Referenced by step().

◆ solveDDP()

RobustDDPController::DDPSolution fsw::control::RobustDDPController::solveDDP	(	const Eigen::VectorXd &	x0,
		const Reference &	ref,
		const Eigen::MatrixXd &	u_init ) const

private

◆ step()

std::pair< RobustDDPController::ActuationCommand, RobustDDPController::Diagnostics > fsw::control::RobustDDPController::step	(	const Measurement &	meas,
		const NavState &	nav,
		const Command &	cmd )

Execute one control step.

Pipeline:

Build state x from measurements
Build reference (F_ref, MR_ref) over horizon
Update robustness bounds
Check supervisory cutoff (§12)
Solve DDP or enumerative MPC (§10)
Safety filter — reachable tube (§9.3)
Actuation quantization + dwell

Definition at line 49 of file RobustDDPController.cpp.

Referenced by engine.pipeline.thermal.wall_conduction.WallModel::advance(), enumerativeMPC(), main(), and engine.pipeline.thermal.wall_conduction.WallModel::soak().

Member Data Documentation

◆ config_

Config fsw::control::RobustDDPController::config_

private

Definition at line 287 of file RobustDDPController.hpp.

Referenced by buildReference(), buildState(), computeActuation(), dynamicsStep(), economicDecision(), enumerativeMPC(), estimateEngine(), initialize(), isStateSafe(), reset(), runningCost(), shouldCutoff(), solveDDP(), and step().

◆ state_

ControllerState fsw::control::RobustDDPController::state_

private

Definition at line 288 of file RobustDDPController.hpp.

Referenced by buildState(), enumerativeMPC(), getState(), isActionSafe(), reset(), RobustDDPController(), solveDDP(), and step().

◆ state_mutex_

std::mutex fsw::control::RobustDDPController::state_mutex_

mutableprivate

Definition at line 289 of file RobustDDPController.hpp.

Referenced by getState(), reset(), and step().

◆ tick_

int fsw::control::RobustDDPController::tick_ = 0

private

Definition at line 294 of file RobustDDPController.hpp.

Referenced by reset(), and step().

◆ u_prev_applied_

std::optional<Eigen::VectorXd> fsw::control::RobustDDPController::u_prev_applied_

private

Definition at line 292 of file RobustDDPController.hpp.

Referenced by enumerativeMPC(), filterAction(), reset(), and step().

◆ u_seq_prev_

std::optional<Eigen::MatrixXd> fsw::control::RobustDDPController::u_seq_prev_

private

Definition at line 293 of file RobustDDPController.hpp.

Referenced by reset(), and step().

◆ x_prev_

std::optional<Eigen::VectorXd> fsw::control::RobustDDPController::x_prev_

private

Definition at line 291 of file RobustDDPController.hpp.

Referenced by reset(), and step().

The documentation for this class was generated from the following files:

daq-server/diablo_server/lib/include/control/RobustDDPController.hpp
daq-server/diablo_server/lib/src/control/RobustDDPController.cpp

Classes

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ CommandType

Constructor & Destructor Documentation

◆ RobustDDPController()

◆ ~RobustDDPController()

Member Function Documentation

◆ buildReference()

◆ buildState()

◆ computeActuation()

◆ dynamicsStep()

◆ economicDecision()

◆ enumerativeMPC()

◆ estimateEngine()

◆ filterAction()

◆ getState()

◆ initialize()

◆ isActionSafe()

◆ isStateSafe()

◆ linearize()

◆ reset()

◆ runningCost()

◆ shouldCutoff()

◆ solveDDP()

◆ step()

Member Data Documentation

◆ config_

◆ state_

◆ state_mutex_

◆ tick_

◆ u_prev_applied_

◆ u_seq_prev_

◆ x_prev_