Diff: ltisys.h

Revision:

0:d144578aa744

Child:

1:231f20f755fe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ltisys.h	Thu May 07 11:10:40 2015 +0000
@@ -0,0 +1,128 @@
+#ifndef LTISYS_H_
+#define LTISYS_H_
+
+template<int nx, int nu, int ny>
+class ltisys{
+public:
+    ltisys(double *matA, double *matB, double *matC, double *matD);
+    virtual ~ltisys();
+
+    inline void update(double dt, double *u);
+    double x[nx], u[nu], 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_ */