eg_plds_ctrl.cpp #
Example PLDS Control ```cpp
//===– eg_plds_ctrl.cpp - Example PLDS Control ———————===// // // Copyright 2021 Michael Bolus // Copyright 2021 Georgia Institute of Technology // // Licensed under the Apache License, Version 2.0 (the “License”); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an “AS IS” BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. // //===———————————————————————-===// //===———————————————————————-===//
#include
using lds::Matrix; using lds::Vector; using lds::data_t; using std::cout;
auto main() -> int { cout « " ********** Example Poisson LDS Control ********** \n\n";
// Make SISO system sampled at 1kHz data_t dt = 1e-3; size_t n_u = 1; size_t n_x = 1; size_t n_y = 1;
// no time steps for simulation. auto n_t = static_cast<size_t>(10.0 / dt);
// Control variables: _reference/target output, controller gains // n.b., Can either use Vector (arma::Col) or std::vector Vector y_ref0 = Vector(n_y, arma::fill::ones) * 30.0 * dt; Matrix k_x = Matrix(n_u, n_x, arma::fill::zeros) + 1; // gains on state error Matrix k_inty = Matrix(n_u, n_y, arma::fill::zeros) + 10; // gains on integrated output err
// Set control type bit mask, so controller knows what to do size_t control_type = lds::kControlTypeIntY; // integral action
// Ground-truth parameters for the controlled system // (stand-in for physical system to be controlled) Matrix a_true(n_x, n_x, arma::fill::eye); a_true[0] = 0.986; Matrix b_true(n_x, n_u, arma::fill::zeros); b_true[0] = 0.054; Vector x0_true = Vector(n_x, arma::fill::ones) * log(1 * dt);
size_t which_m = 0; data_t m_low = log(1 * dt) * (1 - a_true[0]); data_t pr_lo2hi = 1e-3; data_t m_high = log(20 * dt) * (1 - a_true[0]); data_t pr_hi2lo = pr_lo2hi;
Vector m0_true = Vector(n_x, arma::fill::ones) * m_low; // construct ground truth system to be controlled… lds::poisson::System controlled_system(n_u, n_x, n_y, dt);
// Assign params. controlled_system.set_A(a_true); controlled_system.set_B(b_true); controlled_system.set_m(m0_true); controlled_system.set_x0(x0_true); // reset to initial conditions controlled_system.Reset();
cout « “……………………………….\n”; cout « “controlled_system:\n”; cout « “……………………………….\n”; controlled_system.Print(); cout « “……………………………….\n”;
// Create the controller lds::poisson::Controller controller; { // Create model used for control. lds::poisson::System controller_system(controlled_system);
// for this example, assume model correct, except disturbance
Vector m0_controller = Vector(n_x, arma::fill::ones) * m_low;
Vector x0_controller = arma::log(y_ref0);
controller_system.set_m(m0_controller);
controller_system.set_x0(x0_controller);
controller_system.Reset(); //reset to new init condition
// adaptively re-estimate process disturbance (m)
controller_system.do_adapt_m = true;
// set adaptation rate by changing covariance of assumed process noise
// acting on random-walk evolution of m
Matrix q_m = Matrix(n_x, n_x, arma::fill::eye) * 1e-5;
controller_system.set_Q_m(q_m);
data_t u_lb = 0.0;
data_t u_ub = 5.0;
controller = std::move(
lds::poisson::Controller(std::move(controller_system), u_lb, u_ub));
} // set controller type controller.set_control_type(control_type);
// set controller gains controller.set_Kc(k_x); controller.set_Kc_inty(k_inty);
// to protect against integral windup when output is consistently above // target: data_t tau_awu(0.1); controller.set_tau_awu(tau_awu);
cout « “……………………………….\n”; cout « “controller:\n”; cout « “……………………………….\n”; controller.Print(); cout « “……………………………….\n”;
// create Matrix to save outputs in… Matrix y_ref = Matrix(n_y, n_t, arma::fill::zeros); y_ref.each_col() += y_ref0;
// Simulated measurements Matrix z(n_y, n_t, arma::fill::zeros);
// simulated control signal ([=] V) Matrix u(n_u, n_t, arma::fill::zeros);
// outputs, states and gain/disturbance params // *_hat indicates online estimates Matrix y_hat(n_y, n_t, arma::fill::zeros); Matrix x_hat(n_x, n_t, arma::fill::zeros); Matrix m_hat(n_y, n_t, arma::fill::zeros);
// *_true indicates ground truth (system being controlled) Matrix y_true(n_y, n_t, arma::fill::zeros); Matrix x_true(n_x, n_t, arma::fill::zeros); Matrix m_true(n_y, n_t, arma::fill::zeros);
// set initial val y_hat.col(0) = controller.sys().y(); y_true.col(0) = controlled_system.y();
x_hat.col(0) = controller.sys().x(); x_true.col(0) = controlled_system.x();
m_hat.col(0) = controller.sys().m(); m_true.col(0) = controlled_system.m();
cout « “Starting " « n_t * dt « " sec simulation … \n”;
auto start = std::chrono::high_resolution_clock::now();
for (size_t t = 1; t < n_t; t++) {
// simulate a stochastically switched disturbance
Vector chance = arma::randu
// e.g., use sinusoidal reference
data_t f = 0.5; // freq [=] Hz
Vector t_vec = Vector(n_y, arma::fill::ones) * t;
y_ref.col(t) +=
y_ref0 % arma::sin(f * 2 * lds::kPi * dt * t_vec - lds::kPi / 4);
// Simulate the true system.
z.col(t)=controlled_system.Simulate(u.col(t-1));
// This method uses a steady-state solution to control problem to calculate
// x_ref, u_ref from reference output y_ref. Notably, it does this in the
// log-linear space (i.e., log(y)).
//
// Therefore, it is only applicable to regulation problems or cases where
// reference trajectory changes slowly compared to system dynamics.
controller.set_y_ref(y_ref.col(t));
u.col(t)=controller.ControlOutputReference(z.col(t));
y_true.col(t) = controlled_system.y();
x_true.col(t) = controlled_system.x();
m_true.col(t) = controlled_system.m();
y_hat.col(t) = controller.sys().y();
x_hat.col(t) = controller.sys().x();
m_hat.col(t) = controller.sys().m();
}
auto finish = std::chrono::high_resolution_clock::now(); std::chrono::duration<data_t, std::milli> sim_time_ms = finish - start; cout « “Finished simulation in " « sim_time_ms.count() « " ms.\n”; cout « “(app. " « (sim_time_ms.count() / n_t) * 1e3 « " us/time-step)\n”;
// saved variables: dt, y_hat, x_hat, m_hat, z, u, y_ref, y_true, // x_true, m_true saving with hdf5 via armadillo arma::hdf5_opts::opts replace = arma::hdf5_opts::replace;
auto dt_vec = Vector(1).fill(dt); dt_vec.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “dt”)); y_ref.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “y_ref”, replace)); u.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “u”, replace)); z.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “z”, replace)); x_true.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “x_true”, replace)); m_true.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “m_true”, replace)); y_true.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “y_true”, replace)); x_hat.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “x_hat”, replace)); m_hat.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “m_hat”, replace)); y_hat.save(arma::hdf5_name(“eg_plds_ctrl.h5”, “y_hat”, replace));
return 0; }
_Filename: eg_plds_ctrl.cpp_
-------------------------------
Updated on 3 April 2025 at 13:48:30 EDT