File content as of revision 1:231f20f755fe:

#ifndef LTISYS_H_
#define LTISYS_H_

/** ltisys class 
 *  Implementation of continuous-time linear time-invariant system by RK4
 * 
 *  @tparam nx number of states
 *  @tparam nu number of inputs
 *  @tparam ny number of outputs
 */
template<int nx, int nu, int ny>
class ltisys{
public:
    /** Create ltisys instance 
     *  implements
     *    dx/dt = Ax +Bu, 
     *    y = Cx + Du
     * @param matA A-matrix in continuous-time linear state-space representation
     * @param matB B-matrix in continuous-time linear state-space representation
     * @param matC C-matrix in continuous-time linear state-space representation
     * @param matD D-matrix in continuous-time linear state-space representation
     *
     * @note All matrices are row major in this library. Different from MATLAB/FORTRAN.
     */
    ltisys(double *matA, double *matB, double *matC, double *matD);
    virtual ~ltisys();

    /** Update states and outputs
     * updates internal states and outputs of the system
     * calculation is done by 4th order Runge-Kutta method
     * @param dt time elapsed from the last update
     * @param u new input
     */
    inline void update(double dt, double *u);
    
    /// system states
    double x[nx];
    
    /// applied inputs
    double u[nu];
    
    ///  system outputs
    double y[ny];
    
    double A[nx][nx], B[nx][nu], C[ny][nx], D[ny][nu];

private:
    inline void matcopy(int nrow, int ncol, const double *src,
            double *dst);
    inline void mataddprod(int nrowDst, int ncolA, int ncolDst,
            double *matA, double *matB,
            double *dst);
    inline void matprod(int nrowDst, int ncolA, int ncolDst,
            double *matA, double *matB,
            double *dst);
    inline void calc_dx(double *x, double *dx);
};

template <int nx, int nu, int ny>
ltisys<nx,nu,ny>::ltisys(double *matA, double *matB, double *matC, double *matD) {
    matcopy(nx,nx,matA,&A[0][0]);
    matcopy(nx,nu,matB,&B[0][0]);
    matcopy(ny,nx,matC,&C[0][0]);
    matcopy(ny,nu,matD,&D[0][0]);
    for(int i=0;i<nx;i++) x[i]=0.0;
    for(int i=0;i<nu;i++) u[i]=0.0;
    for(int i=0;i<ny;i++) y[i]=0.0;
}

template <int nx, int nu, int ny>
ltisys<nx,nu,ny>::~ltisys() {
}


template <int nx, int nu, int ny>
inline void ltisys<nx,nu,ny>::update(double dt, double *new_u){
    double k1[nx],k2[nx],k3[nx],k4[nx],xt[nx];

    // update states by rk4 method
    // k1=Ax+Bu
    calc_dx(x,k1);

    // k2
    for(int i=0;i<nx;i++){
        xt[i]=x[i]+dt*k1[i]/2;
    }
    calc_dx(xt,k2);

    // k3
    for(int i=0;i<nx;i++){
        xt[i]=x[i]+dt*k2[i]/2;
    }
    calc_dx(xt,k3);

    // k4
    for(int i=0;i<nx;i++){
        xt[i]=x[i]+dt*k3[i];
    }
    calc_dx(xt,k4);

    for(int i=0; i<nx; i++){
        x[i] += dt*(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6;
    }

    // update I/O
    matprod(ny,nx,1,&C[0][0],&x[0],&y[0]);
    matcopy(nu,1,new_u,u);
    mataddprod(ny,nu,1,&D[0][0],&u[0],&y[0]);
}

template <int nx, int nu, int ny>
inline void ltisys<nx,nu,ny>::matcopy(int nrow, int ncol, const double *src,
        double *dst) {
    for (int i = 0; i < nrow*ncol; i++)
            dst[i] = src[i];
}

template <int nx, int nu, int ny>
inline void ltisys<nx,nu,ny>::mataddprod(int nrowDst, int ncolA, int ncolDst,
        double *matA, double *matB,
        double *dst) {
    for (int i = 0; i < nrowDst; i++) {
        for (int j = 0; j < ncolDst; j++) {
            double prod = 0.0;
            for (int k = 0; k < ncolA; k++) {
                prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
            }
            dst[i*ncolDst+j] += prod;
        }
    }
}

template <int nx, int nu, int ny>
inline void ltisys<nx,nu,ny>::matprod(int nrowDst, int ncolA, int ncolDst,
        double *matA, double *matB,
        double *dst) {
    for (int i = 0; i < nrowDst; i++) {
        for (int j = 0; j < ncolDst; j++) {
            double prod = 0.0;
            for (int k = 0; k < ncolA; k++) {
                prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
            }
            dst[i*ncolDst+j] = prod;
        }
    }
}

template <int nx, int nu, int ny>
inline void ltisys<nx,nu,ny>::calc_dx(double *x, double *dx) {
    for (int i = 0; i < nx; i++) {
        dx[i] = 0.0;
        for (int j = 0; j < nx; j++) {
            dx[i] += A[i][j] * x[j];
        }
        for (int j = 0; j < nu; j++) {
            dx[i] += B[i][j] * u[j];
        }
    }
}

#endif /* LTISYS_H_ */