ltisys.h

00001 /* ltisys
00002  * implementation of continuous-time time-invariant linear systems
00003  * Copyright (c) 2015 Ichiro Maruta
00004  * Released under the MIT License: http://opensource.org/licenses/mit-license.php
00005  */
00006 
00007 #ifndef LTISYS_H_
00008 #define LTISYS_H_
00009 
00010 /** ltisys class 
00011  *  Implementation of continuous-time linear time-invariant system by RK4
00012  * 
00013  *  @tparam nx number of states
00014  *  @tparam nu number of inputs
00015  *  @tparam ny number of outputs
00016  */
00017 template<int nx, int nu, int ny>
00018 class ltisys{
00019 public:
00020     /** Create ltisys instance 
00021      *  implements
00022      *    dx/dt = Ax +Bu, 
00023      *    y = Cx + Du
00024      * @param matA A-matrix in continuous-time linear state-space representation
00025      * @param matB B-matrix in continuous-time linear state-space representation
00026      * @param matC C-matrix in continuous-time linear state-space representation
00027      * @param matD D-matrix in continuous-time linear state-space representation
00028      *
00029      * @note All matrices are row major in this library. Different from MATLAB/FORTRAN.
00030      */
00031     ltisys(const double *matA, const double *matB, const double *matC, const double *matD);
00032     virtual ~ltisys();
00033 
00034     /** Update states and outputs
00035      * updates internal states and outputs of the system
00036      * calculation is done by 4th order Runge-Kutta method
00037      * @param dt time elapsed from the last update
00038      * @param u new input
00039      */
00040     inline void update(double dt, double *u);
00041     
00042     /// system states
00043     double x[nx];
00044     
00045     /// applied inputs
00046     double u[nu];
00047     
00048     ///  system outputs
00049     double y[ny];
00050     
00051     const double *A, *B, *C, *D;
00052 
00053 private:
00054     inline void matcopy(int nrow, int ncol, const double *src,
00055             double *dst);
00056     inline void mataddprod(int nrowDst, int ncolA, int ncolDst,
00057             const double *matA, const double *matB,
00058             double *dst);
00059     inline void matprod(int nrowDst, int ncolA, int ncolDst,
00060             const double *matA, const double *matB,
00061             double *dst);
00062     inline void calc_dx(double *x, double *dx);
00063 };
00064 
00065 template <int nx, int nu, int ny>
00066 ltisys<nx,nu,ny>::ltisys(const double *matA, const double *matB, const double *matC, const double *matD): A(matA), B(matB), C(matC), D(matD){
00067     for(int i=0;i<nx;i++) x[i]=0.0;
00068     for(int i=0;i<nu;i++) u[i]=0.0;
00069     for(int i=0;i<ny;i++) y[i]=0.0;
00070 }
00071 
00072 template <int nx, int nu, int ny>
00073 ltisys<nx,nu,ny>::~ltisys() {
00074 }
00075 
00076 
00077 template <int nx, int nu, int ny>
00078 inline void ltisys<nx,nu,ny>::update(double dt, double *new_u){
00079     double k1[nx],k2[nx],k3[nx],k4[nx],xt[nx];
00080 
00081     // update states by rk4 method
00082     // k1=Ax+Bu
00083     calc_dx(x,k1);
00084 
00085     // k2
00086     for(int i=0;i<nx;i++){
00087         xt[i]=x[i]+dt*k1[i]/2;
00088     }
00089     calc_dx(xt,k2);
00090 
00091     // k3
00092     for(int i=0;i<nx;i++){
00093         xt[i]=x[i]+dt*k2[i]/2;
00094     }
00095     calc_dx(xt,k3);
00096 
00097     // k4
00098     for(int i=0;i<nx;i++){
00099         xt[i]=x[i]+dt*k3[i];
00100     }
00101     calc_dx(xt,k4);
00102 
00103     for(int i=0; i<nx; i++){
00104         x[i] += dt*(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6;
00105     }
00106 
00107     // update I/O
00108     matprod(ny,nx,1,C,&x[0],&y[0]);
00109     matcopy(nu,1,new_u,u);
00110     mataddprod(ny,nu,1,D,&u[0],&y[0]);
00111 }
00112 
00113 template <int nx, int nu, int ny>
00114 inline void ltisys<nx,nu,ny>::matcopy(int nrow, int ncol, const double *src,
00115         double *dst) {
00116     for (int i = 0; i < nrow*ncol; i++)
00117             dst[i] = src[i];
00118 }
00119 
00120 template <int nx, int nu, int ny>
00121 inline void ltisys<nx,nu,ny>::mataddprod(int nrowDst, int ncolA, int ncolDst,
00122         const double *matA, const double *matB,
00123         double *dst) {
00124     for (int i = 0; i < nrowDst; i++) {
00125         for (int j = 0; j < ncolDst; j++) {
00126             double prod = 0.0;
00127             for (int k = 0; k < ncolA; k++) {
00128                 prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
00129             }
00130             dst[i*ncolDst+j] += prod;
00131         }
00132     }
00133 }
00134 
00135 template <int nx, int nu, int ny>
00136 inline void ltisys<nx,nu,ny>::matprod(int nrowDst, int ncolA, int ncolDst,
00137         const double *matA, const double *matB,
00138         double *dst) {
00139     for (int i = 0; i < nrowDst; i++) {
00140         for (int j = 0; j < ncolDst; j++) {
00141             double prod = 0.0;
00142             for (int k = 0; k < ncolA; k++) {
00143                 prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
00144             }
00145             dst[i*ncolDst+j] = prod;
00146         }
00147     }
00148 }
00149 
00150 template <int nx, int nu, int ny>
00151 inline void ltisys<nx,nu,ny>::calc_dx(double *x, double *dx) {
00152     for (int i = 0; i < nx; i++) {
00153         dx[i] = 0.0;
00154         for (int j = 0; j < nx; j++) {
00155             dx[i] += A[i*nx+j] * x[j];
00156         }
00157         for (int j = 0; j < nu; j++) {
00158             dx[i] += B[i*nu+j] * u[j];
00159         }
00160     }
00161 }
00162 
00163 #endif /* LTISYS_H_ */