/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/dynamics/LinearModel.cc

Go to the documentation of this file.

/*
 * LinearModel.cpp
 *
 *  Created on: Apr 28, 2009
 *      Author: jgoppert
 */

#include "LinearModel.h"

using namespace boost::numeric::ublas;

namespace uvsim
{

LinearModel::LinearModel(vector<double> *x, vector<double> *u) :
        Model(x,u)
{
}

LinearModel::~LinearModel()
{
}

void LinearModel::print()
{
    std::cout << "\nContinuous Process Matrices:"
                      << "\nAc: " << Ac
              << "\nBc: " << Bc
              << "\nEc: " << Ec << std::endl;
    std::cout << "\nDiscrete Process Matrices:"
                      << "\nAd: " << Ad
              << "\nBd: " << Bd
              << "\nEd: " << Ed << std::endl;
    std::cout << "\nMeasurement Matrices:"
                      << "\nC: " << C 
              << "\nL: " << L  << std::endl;
}

void LinearModel::size(int n, int m, int p)
{
    this->n = n;
    this->m = m;
    this->p = p;
    Ac = zero_matrix<double>(n,n);
    Bc = zero_matrix<double>(n,p);
    Ec= zero_matrix<double>(n,n);
        Ad = zero_matrix<double>(n,n);
    Bd = zero_matrix<double>(n,p);
    Ed= zero_matrix<double>(n,n);
    C = zero_matrix<double>(m,n);
    L = zero_matrix<double>(m,m);
}

void LinearModel::discretize(double freq, int order)
{
        double dt = 1/freq;
        Ad = identity_matrix<double>(n);
        matrix<double> X = identity_matrix<double>(n)*dt;
        double num = 1, den = 1;
        matrix<double> Apow = identity_matrix<double>(n);
        for (int i=1;i<=order;i++)
        {
                num *= dt;
                den *= i;
                Apow = prod(Apow, Ac);
                Ad = Ad + Apow*num/den;
                X = X + Apow*dt*num/((i+1)*den);
        }
        Bd = prod(X,Bc);
        Ed = prod(X,Ec);
}

}

// vim:ts=4:sw=4

---