Example: Gradient Estimator

Consider the linear regression equation (LRE) [1]:

\[y(t) = \phi^\intercal(t) \theta\]

C++Python

#include <Eigen/Core>
#include <chrono>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

#include "sdu_estimators/parameter_estimators/gradient_estimator.hpp"
#include "sdu_estimators/integrator/integrator.hpp"

using namespace sdu_estimators;

#define DIM_N 4
#define DIM_P 2

int main()
{
    float dt = 0.001;
    float tend = 50 / dt; // 50s

    Eigen::Vector<double, DIM_P> gamma = {1, 1};
    gamma = 0.1 * gamma;
    float r = 0.5;
    Eigen::Vector<double, DIM_P> theta_init, theta_true;

    theta_init << 0,
                  0;
    theta_true << 1,
                  2;

    integrator::IntegrationMethod intg_method = integrator::IntegrationMethod::RK4;
    parameter_estimators::GradientEstimator<double, DIM_N, DIM_P> solver(dt, gamma, theta_init, r, intg_method);
    std::vector<Eigen::VectorXd> all_theta_est;
    Eigen::Vector<double, DIM_N> y;
    Eigen::Vector<double, DIM_P, DIM_N> phi;

    float t;

    for (int i = 0; i < tend; ++i)
    {
        t = i * dt;
        phi << 2.*std::cos(t), -std::cos(t+1.), 3.*std::cos(2.*t+1./2.), 2.*std::cos(t/3. + 1.),
            std::cos(2.*t), std::cos(t/2.), 2.*std::cos(3.*t/2. + 3./4.), -3.*std::cos(4.*t/3.);
        y << phi.transpose() * theta_true;

        grad_est.step(y, phi);

        Eigen::VectorXd tmp = solver.get_estimate();

        // save data
        all_theta_est.push_back(tmp);
    }

    // Write all_theta_est to file
    std::ofstream outfile;
    outfile.open ("data_gradient.csv");

    outfile << "timestamp,theta_est_1,theta_est_2,theta_act_1,theta_act_2" << std::endl;

    for (int i = 0; i < tend; ++i)
    {
        outfile << i * dt << "," << all_theta_est[i][0] << "," << all_theta_est[i][1]
                << "," << theta_true[0] << "," << theta_true[1] << std::endl;
    }

    outfile.close();
}