eg_plds_est.cpp

eg_plds_est.cpp #

Example PLDS Estimation ```cpp

//===– eg_plds_est.cpp - Example PLDS Estimation ————————-===// // // 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;

// for generating random input Matrix random_walk(size_t n_t, const Matrix& Q, const Vector& x0);

int main() { cout « " ********** Example Poisson LDS Estimation ********** \n\n";

// Make SISO system sampled at 1kHz data_t dt = 1e-3; size_t n_u = 1; // no. inputs size_t n_x = 1; // no. states size_t n_y = 1; // no. outputs auto n_t = static_cast<size_t>(30 / dt); // no time steps for simulation.

// construct ground truth system… lds::poisson::System system_true(n_u, n_x, n_y, dt);

// Model parameters Matrix a_true(n_x, n_x, arma::fill::eye); a_true[0] = exp(-dt / 0.075); Matrix b_true = Matrix(n_x, n_u).fill(1e-2); Vector m0_true = Vector(n_x, arma::fill::zeros).fill(-7e-2); // disturbance Vector x0_true = m0_true * arma::inv(Matrix(n_x, n_x, arma::fill::eye) - a_true); // initial state

// Assign params. system_true.set_A(a_true); system_true.set_B(b_true); system_true.set_x0(x0_true); system_true.set_m(m0_true); system_true.Reset();

// Construct system for estimation // e.g., will create a model with incorrect disturbance lds::poisson::System system_estimator(n_u, n_x, n_y, dt);

// Can copy parameters from another system object system_estimator = system_true;

// wrong disturbance Vector m0_est = m0_true * 2; system_estimator.set_m(m0_est);

// set new initial conditions Vector x0_est = m0_est * arma::inv(Matrix(n_x, n_x, arma::fill::eye) - a_true); // initial state system_estimator.set_x0(x0_est); system_estimator.Reset(); // reset to initial condition.

// turn on adaptive disturbance estimation system_estimator.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-6; system_estimator.set_Q_m(q_m);

cout « “……………………………….\n”; cout « “estimator:\n”; cout « “……………………………….\n”; system_estimator.Print(); cout « “……………………………….\n”;

// Set up simulation : // Simulated measurements Matrix z(n_y, n_t, arma::fill::zeros);

// Stimulus (generate random stimulus) Matrix q_u = Matrix(n_u, n_u, arma::fill::eye) * 1e-3; // cov of random walk Matrix u = random_walk(n_t, q_u, Vector(n_u, arma::fill::zeros));

// create matrix to save outputs in… Matrix y_hat(n_y, n_t, arma::fill::zeros); Matrix y_true(n_y, n_t, arma::fill::zeros);

// states and disturbance params Matrix x_hat(n_x, n_t, arma::fill::zeros); Matrix m_hat(n_x, n_t, arma::fill::zeros);

Matrix x_true(n_x, n_t, arma::fill::zeros); Matrix m_true(n_x, n_t, arma::fill::zeros);

// initial conditions y_hat.col(0) = system_estimator.y(); y_true.col(0) = system_true.y(); x_hat.col(0) = system_estimator.x(); x_true.col(0) = system_true.x(); m_hat.col(0) = system_estimator.m(); m_true.col(0) = system_true.m();

cout « “Starting " « n_t * dt « " sec simlation … \n”; auto start = std::chrono::high_resolution_clock::now(); for (size_t t = 1; t < n_t; t++) { // Simlate the true system. z.col(t) = system_true.Simulate(u.col(t - 1));

// Filter (predict -> update)
system_estimator.Filter(u.col(t - 1), z.col(t));

// save signals
y_hat.col(t) = system_estimator.y();
y_true.col(t) = system_true.y();

x_true.col(t) = system_true.x();
m_true.col(t) = system_true.m();

x_hat.col(t) = system_estimator.x();
m_hat.col(t) = system_estimator.m();

}

auto finish = std::chrono::high_resolution_clock::now(); std::chrono::duration<data_t, std::milli> sim_time_ms = finish - start; cout « “Finished simlation 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_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_est.h5”, “dt”)); u.save(arma::hdf5_name(“eg_plds_est.h5”, “u”, replace)); z.save(arma::hdf5_name(“eg_plds_est.h5”, “z”, replace)); x_true.save(arma::hdf5_name(“eg_plds_est.h5”, “x_true”, replace)); m_true.save(arma::hdf5_name(“eg_plds_est.h5”, “m_true”, replace)); y_true.save(arma::hdf5_name(“eg_plds_est.h5”, “y_true”, replace)); x_hat.save(arma::hdf5_name(“eg_plds_est.h5”, “x_hat”, replace)); m_hat.save(arma::hdf5_name(“eg_plds_est.h5”, “m_hat”, replace)); y_hat.save(arma::hdf5_name(“eg_plds_est.h5”, “y_hat”, replace));

return 0; }

// for generating random input Matrix random_walk(size_t n_t, const Matrix& Q, const Vector& x0) { size_t n = Q.n_rows;

if ((n != Q.n_cols) || (Q.n_cols != Q.n_rows)) { throw std::logic_error(“Q must be n x n.”); }

Matrix x(n, n_t, arma::fill::zeros); x.col(0) = x0; for (size_t t = 1; t < n_t; t++) { x.col(t) = x.col(t - 1) + arma::mvnrnd(Vector(n, arma::fill::zeros), Q); }

return x; }


_Filename: eg_plds_est.cpp_

-------------------------------

Updated on  3 April 2025 at 13:48:30 EDT